CN110221538B - 一种结合迭代学习的时间最优轨迹规划控制器及方法 - Google Patents

一种结合迭代学习的时间最优轨迹规划控制器及方法 Download PDF

Info

Publication number
CN110221538B
CN110221538B CN201910341967.9A CN201910341967A CN110221538B CN 110221538 B CN110221538 B CN 110221538B CN 201910341967 A CN201910341967 A CN 201910341967A CN 110221538 B CN110221538 B CN 110221538B
Authority
CN
China
Prior art keywords
path
joint
discrete point
vector
iterative learning
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.)
Expired - Fee Related
Application number
CN201910341967.9A
Other languages
English (en)
Other versions
CN110221538A (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.)
South China University of Technology SCUT
Zhongshan Institute of Modern Industrial Technology of South China University of Technology
Original Assignee
South China University of Technology SCUT
Zhongshan Institute of Modern Industrial Technology of South China University of Technology
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 South China University of Technology SCUT, Zhongshan Institute of Modern Industrial Technology of South China University of Technology filed Critical South China University of Technology SCUT
Priority to CN201910341967.9A priority Critical patent/CN110221538B/zh
Publication of CN110221538A publication Critical patent/CN110221538A/zh
Application granted granted Critical
Publication of CN110221538B publication Critical patent/CN110221538B/zh
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance

Landscapes

  • Engineering & Computer Science (AREA)
  • Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Computation (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Feedback Control In General (AREA)
  • Numerical Control (AREA)

Abstract

本发明公开了一种结合迭代学习的时间最优轨迹规划控制器及方法，所述控制器包括路径离散模块、轨迹规划模块、迭代学习模块和存储模块；路径离散模块，用于将任务路径离散化；轨迹规划模块，获得时间最优轨迹；迭代学习模块用于补偿动力学模型误差；存储模块存储每次迭代的迭代数据。所述方法包括以下步骤：S1、将连续任务路径输入到路径离散模块将路径离散化;S2、将路径离散化后的结果输入到轨迹规划模块，获得时间最优轨迹;S3、运行轨迹获得反馈力矩;S4、将计算结果和存储模块中的迭代数据输入到迭代学习模块中，输出迭代学习补偿项;S5、更新关节空间动力学模型；S6、重复步骤S2‑步骤S5直到动力学模型不再更新。

Description

一种结合迭代学习的时间最优轨迹规划控制器及方法
技术领域
本发明涉及机器人控制器及方法，尤其是一种结合迭代学习的时间最优轨迹规划控制器及方法。
背景技术
在利用机器人进行搬运，装配，机加工等场合时，为了提高机器人的工作效率，应该使机器人始终处于允许的最大速度下工作，这种为了减少机器人执行任务的时间，使机器人在临界条件下运动的轨迹规划方法称为时间最优轨迹规划方法。
在使用动力学模型进行最优轨迹规划时，由于动力学模型并不是完全准确的，这导致了不可避免的模型工厂不匹配问题，造成通过动力学约束求得的时间最优轨迹不是最优解。
发明内容
本发明的目的是提供一种结合迭代学习的时间最优轨迹规划控制器及方法，通过迭代学习方法消除模型工厂不匹配(model-plant mismatch)问题，最终使时间最优轨迹接近最优解，同时通过准确的预测力矩进行力矩前馈控制，提高机器人高速状态下的跟踪精度，用于生成机器人沿着给定路径运行时操作时间最短的控制指令以及提高跟踪时的跟踪性能。
本发明采用以下的技术方案。
一种结合迭代学习的时间最优轨迹规划控制器，所述轨迹规划控制器包括路径离散模块、轨迹规划模块、迭代学习模块和存储模块；
路径离散模块，用于将机器人的任务路径离散化；
轨迹规划模块用于获得时间最优轨迹，具体是根据约束条件，建立每个离散点的关节空间动力学模型，并将关节空间动力学模型转化到路径空间动力学模型并构建非线性优化模型，通过非线性求解工具求得机器人的时间最优轨迹，从而求得最优路径-时间函数，将控制周期代入最优位移-时间函数得到每个控制周期的最优位移，从而得到每个控制周期各关节的角度，并根据电机每圈脉冲数以及减速器传动比将每个控制周期各关节角度并转化为每个控制周期的脉冲指令，同时计算相应的力矩作为前馈力矩，并将脉冲指令和前馈力矩用于机器人的控制；
迭代学习模块用于补偿动力学模型误差，提高动力学模型精度，从而获得最优解和提高跟踪性能，具体是利用轨迹运行后获得的反馈力矩和前馈力矩以及存储模块中的上一次迭代的力矩偏差和迭代学习补偿项输入迭代学习控制器来获得新的迭代学习补偿项，用于补偿关节空间动力学模型的建模误差，从而更新轨迹规划模块中的动力学模型，用于轨迹规划模块进行轨迹规划；
存储模块用于存储每次迭代的迭代数据，以用于下一次迭代；迭代数据包括前馈力矩、反馈力矩、力矩偏差和迭代学习补偿项。
一种结合迭代学习的时间最优轨迹规划方法，包括以下步骤：
S1.通过离线规划或利用示教器获得机器人的连续任务路径，并将连续任务路径输入到路径离散模块将路径离散化；
S2.将路径离散化后的结果输入到轨迹规划模块，获得时间最优轨迹，并转化为相应的脉冲指令，再将脉冲指令和前馈力矩下发到机器人控制柜；
S3.运行轨迹获得反馈力矩；
S4.计算前馈力矩和反馈力矩的力矩偏差，将计算结果和存储模块中的迭代数据输入到迭代学习模块中更新迭代学习补偿项，输出新的迭代学习补偿项；
S5.利用新的迭代学习补偿项更新迹规划模块中的关节空间动力学模型，用于轨迹规划模块中重新规划和运行；
S6.重复步骤S2-步骤S5直到关节空间动力学模型不再更新。
进一步的，步骤S1的路径离散模块将连续的任务路径离散为N个离散点，并假设相邻两点之间为匀加速运动。
进一步的，步骤S2具体包括以下步骤：
S21.建立每个离散点的机器人关节空间动力学模型，并根据动力学约束条件建立关节空间的力矩约束不等式方程组以及根据运动学约束条件建立速度约束不等式方程组和加速度约束不等式方程组；
S22.将步骤S21得到的关节空间的约束不等式方程组转化为路径空间的约束不等式方程组，并根据优化目标，构建非线性优化模型；
S23.根据非线性优化模型通过非线性求解工具求解机器人的时间最优轨迹；非线性求解工具包括python的CasADi、matlab的yamlip、sedumi和fmincon等；
S24.根据时间最优轨迹得到最优位移-时间函数，将控制周期代入最优位移时间函数得到每个控制周期的最优位移，从而得到每个控制周期各关节角度，并根据电机每圈脉冲数以及减速器传动比将每个控制周期各关节角度转化为每个控制周期的脉冲指令，同时计算相应的力矩作为前馈力矩,用于后续控制。
进一步的，步骤S21具体包括如下：
建立如下的关节空间动力学模型的基础形式：
Figure BDA0002041011900000031
其中，τ表示n×1阶关节力矩矢量，即[τ1τ2…τn]T,其中n表示机器人关节数，τn表示第n个关节的力矩矢量，上标T表示转置，M(q)表示n×n阶正定质量矩阵；q表示n×1阶关节角度矢量，即[q1q2…qn]，qn表示第n个关节的矢量；
Figure BDA0002041011900000032
表示n×1阶关节加速度矢量，即
Figure BDA0002041011900000033
Figure BDA0002041011900000034
表示第n个关节的加速度矢量；B(q)表示n×n(n-1)/2阶的哥氏力系数矩阵，
Figure BDA0002041011900000035
表示n(n-1)/2×1阶的关节速度积矢量，即
Figure BDA0002041011900000036
Figure BDA0002041011900000037
表示n×1阶关节速度矢量，即
Figure BDA0002041011900000041
Figure BDA0002041011900000042
表示第n个关节的速度矢量；C(q)是n×n阶离心力系数矩阵，而
Figure BDA0002041011900000043
是n×1阶矢量，即
Figure BDA0002041011900000044
Fv表示n×1阶粘滞摩擦力向量；Fc表示n×1阶库伦摩擦力向量；G(q)表示重力向量；δl(q)是n×1阶迭代学习补偿向量，即[δ1,l(q1)δ2,l(q2)…δn,l(qn)]T,l为迭代次数，在没有迭代即l＝0时δl(q)设置为n×1阶0向量；sign表示符号函数；
根据式(1)建立离散点k的关节空间动力学模型：
Figure BDA0002041011900000045
其中，k表示离散点，N表示总的离散点数，τ(k)表示n×1阶关节力矩矢量，即[τ1(k)τ2(k)…τn(k)]T,其中n表示机器人关节数，τn(k)表示第n个关节的力矩矢量，上标T表示转置，M(q(k))表示n×n阶正定质量矩阵；q(k)表示n×1阶关节角度矢量，即[q1(k)q2(k)…qn(k)]，qn(k)表示第n个关节的矢量；
Figure BDA0002041011900000046
表示n×1阶关节加速度矢量，即
Figure BDA0002041011900000047
Figure BDA0002041011900000048
表示第n个关节的加速度矢量；B(q(k))表示n×n(n-1)/2阶的哥氏力系数矩阵，
Figure BDA0002041011900000049
表示n(n-1)/2×1阶的关节速度积矢量，即
Figure BDA00020410119000000410
Figure BDA00020410119000000411
表示n×1阶关节速度矢量，即
Figure BDA00020410119000000412
Figure BDA00020410119000000413
表示第n个关节的速度矢量；C(q(k))是n×n阶离心力系数矩阵，而
Figure BDA00020410119000000414
是n×1阶矢量，即
Figure BDA00020410119000000415
Fv表示粘滞摩擦力向量；Fc表示库伦摩擦力向量；G(q(k))表示重力向量；δl(q(k))是n×1阶迭代学习补偿向量，即[δ1,l(q1(k))δ2,l(q(k))…δn,l(q(k))]T,l为迭代次数，在没有迭代即l＝0时δl(q(k))设置为n×1阶0向量；sign表示符号函数；
设定动力学约束条件为：
τmin(k)≤τ(k)≤τmax(k)
其中，τmin(k)表示离散点k的最小约束力矩，τmax(k)表示离散点k的最大约束力矩；
设定运动学约束条件为：
Figure BDA0002041011900000051
Figure BDA0002041011900000052
其中，
Figure BDA0002041011900000053
表示离散点k的最小约束速度，
Figure BDA0002041011900000054
表示离散点k的最大约束速度，
Figure BDA0002041011900000055
表示离散点k的最小约束加速度，
Figure BDA0002041011900000056
表示离散点k的最大约束加速度；
从而得到关节空间的约束不等式方程组为：
Figure BDA0002041011900000057
进一步的，步骤S22具体包括如下：
根据链式规则，将离散点k的速度
Figure BDA0002041011900000058
和加速度
Figure BDA0002041011900000059
分解为如下的路径空间中路径速度和路径加速度的方程：
Figure BDA00020410119000000510
Figure BDA00020410119000000511
上式中，为了与关节空间区分开来，将路径空间的关节角度q(s(k))表示，其与q(k)是等价的，也表示在路径位移为s(k)时，路径函数q(s)的值；关节速度用
Figure BDA00020410119000000512
表示，其与
Figure BDA00020410119000000513
是等价的；将关节加速度用
Figure BDA00020410119000000514
表示，其与
Figure BDA00020410119000000515
是等价的；
Figure BDA00020410119000000516
是路径速度，是路径位移s(k)对时间t的一阶导数；
Figure BDA00020410119000000517
是路径加速度，是路径位移s(k)对时间t的二阶导数；q′(s(k))是关节角度q(s(k))对路径位移的一阶导，即路径位移为s(k)时，路径函数q(s)对路径位移的一阶微分函数q′(s)的值；
Figure BDA00020410119000000518
是关节角度q(s(k))对路径位移的二阶导，即路径位移为s(k)时，路径函数q(s)对路径位移的二阶微分函数q″(s)的值；
将式(4)、(5)代入式(2)、(3)得到路径空间的约束不等式方程组
Figure BDA0002041011900000061
其中
Figure BDA0002041011900000062
上述式(7)为关节空间动力学模型，是将式(4)和(5)代入关节空间动力学模型(2)得到的；
将式(7)简化为如下的路径空间动力学模型为：
Figure BDA0002041011900000063
其中
m(s(k))＝M(q(s(k)))q′(s(k))
c(s(k))＝M(q(s(k)))q″(s(k))+B(q(s(k))，q′(s(k)))q′(s(k))+C(q(s(k)))(q′(s(k)))2
f(s(k))＝Fv(q(s(k)))q′(s(k))
g(s(k))＝G(q(s(k)))
δl(s(k))＝δl(q(k))
上式中M(q(s(k)))、B(q(s(k)),q′(s(k)))、C(q(s(k)))、Fv(q(s(k)))、Fc(q(s(k)))、G(q(s(k)))是用来简化路径空间动力学模型的中间变量
所述优化目标为
min T
其中T表示运行任务路径所需要的时间，由于
Figure BDA0002041011900000071
Figure BDA0002041011900000072
为了计算方便，将优化目标用下式表示
Figure BDA0002041011900000073
所述非线性模型为
Figure BDA0002041011900000074
Figure BDA0002041011900000075
其中s.t.表示的是约束条件符号，s(1)＝0表示路径起点离散点1的路径位移为0，s(N)＝1表示路径终点离散点N的路径位移为1，
Figure BDA0002041011900000076
表示路径起点离散点1的路径速度为0，
Figure BDA0002041011900000077
表示路径终点离散点N的路径速度为0，δl(s(k)表示路径位移第l次迭代的迭代学习补偿项；
进一步的，所述最优路径-时间函数通过以下计算得到：
根据
Figure BDA0002041011900000078
的转换关系，得到到达每个离散点K所需要的运行时间：
Figure BDA0002041011900000079
其中k表示路径中的第k个离散点，t(k)表示从轨迹起点开始到第k个离散点所需要的时间；
通过分段最小二乘法拟合每个离散点的时间t(k)以及每个离散点处的路径位移s(k)的关系，得到连续的最优位移-时间函数s(t)；
所述前馈力矩的计算方法为：
将最优位移-时间函数s(t)对时间t求一阶导得到最优速度-时间函数
Figure BDA0002041011900000081
将最优位移-时间函数s(t)对时间t求二阶导得到最优加速度-时间函数
Figure BDA0002041011900000082
将s(t)，
Figure BDA0002041011900000083
代入式(6)得最优轨迹的力矩-时间函数τ(s(t))，将控制周期T控制周期代入τ(s(t))得到每个控制周期的力矩，即为前馈力矩。
进一步的，步骤S4所述包括以下步骤：
S41.将获得的测量力矩离散化；
S42.计算前馈力矩和反馈力矩的力矩偏差，将获得的力矩偏差和存储模块中上一次迭代的力矩偏差以及迭代学习补偿项输入到迭代学习模块的迭代学习控制器中，更新迭代学习补偿项；
S43.将更新后的迭代学习补偿项平滑滤波处理后再添加到轨迹规划模块中，用于更新轨迹规划模块中的关节空间动力学模型，重新轨迹进行规划。
进一步的，迭代学习控制器中的迭代学习算法如下：
δi,l(s(k))＝γi,lΔτi,l(s(k))+δi,l-1(s(k)) (6)
其中i＝1…n表示机器人的关节编号，Δτi,l(s(k))＝τi,反馈(s(k))-τi,前馈(s(k))表示力矩偏差，其中τi,反馈(s(k))表示第k个离散点第i个机器人关节的反馈力矩，τi,前馈(s(k))是第k个离散点第i个机器人关节的前馈力矩；l表示迭代次数，γi,l是第i个机器人关节第l次迭代的收敛因子，用于控制迭代学习的收敛，δi,l-1(s(k))是第k个离散点第i个机器人关节第l-1次迭代的迭代学习补偿项，用于保存此前的学习行为；
其中，为了使迭代收敛，收敛因子γi,l的计算公式如下：
Figure BDA0002041011900000091
其中γi,l表示第i个机器人关节第l次迭代的收敛因子，Δτi,l-1(s(k))表示第k个离散点第i个机器人关节第l-1次迭代的力矩偏差，Δτi,l(s(k))表示第k个离散点第i个机器人关节第l次迭代的力矩偏差，
Figure BDA0002041011900000092
表示欧几里得范数的平方，
Figure BDA0002041011900000093
表示从括号中的数从1到N进行求和；
设l′是有效迭代次数，表示当力矩偏差成功减小时的迭代次数，根据γi,l的计算结果，迭代规则分为以下两种情况：
Case1.如果γi,l≥0，令l′＝l，返回式(6)进行迭代；收敛因子γi,l等于0，作为迭代终止的标志即关节空间动力学模型不再更新；；
Case2.如果γi,l<0,令γi,l＝0.5γi,l′，γi,l′＝0.5γi,l′，Δτi,l(s(k))＝Δτi,l′(s(k)),δi,l-1(s(k))＝δi,l′-1(s(k))，返回式(6)进行迭代，其中，γi,l′表示第l′次迭代，第i关节的收敛因子，Δτi,l′(s(k))表示第l′次迭代，第i关节，第k个离散点的力矩偏差，δi,l′-1(s(k))表示第l′-1次迭代，第i关节，第k个离散点的迭代学习补偿项。
进一步的，步骤S43所述的平滑滤波处理如下：
Figure BDA0002041011900000094
其中a表示滤波步长。
与现有技术相比，本发明的有益效果是：通过迭代学习算法，改善了动力学模型的误差，使用于时间最优规划的动力学模型更准确，从而获得更接近最优解的时间最优规划轨迹，并且准确的动力学模型作为前馈力矩进行控制时，可以提高机器人的跟踪性能。
附图说明
图1为实施例中一种结合迭代学习的时间最优轨迹规划方法的流程图；
图2为本实施例的机器人装置系统以及机器人的任务路径图；
图3为本实施例的机器人装置系统的设备简图；
其中：201-关节机器人、202-机器人控制柜、203-示教器、301-外部存储设备、302-只读存储器、303-示教板、304-伺服驱动器、305-电机。
具体实施方式
下面通过具体实例对本发明的目的作进一步详细地描述，实例不能在此一一赘述，但本发明的实施方式并不因此限定于以下实例。
本实施例的一种结合迭代学习的时间最优轨迹规划控制器，所述轨迹规划控制器包括路径离散模块、轨迹规划模块、迭代学习模块和存储模块；
路径离散模块，用于将机器人的任务路径离散化；
轨迹规划模块用于获得时间最优轨迹，具体是根据约束条件，建立每个离散点的关节空间动力学模型，并将关节空间动力学模型转化到路径空间动力学模型并构建非线性优化模型，通过非线性求解工具求解机器人的时间最优轨迹，从而求得最优速度时间函数，对最优速度时间函数积分得到最优位移时间函数，将控制周期代入最优位移-时间函数得到每个控制周期的最优位移，从而得到每个控制周期各关节的角度，并根据电机参数以及减速器传动比将每个控制周期各关节角度并转化为每个控制周期的脉冲指令，同时计算相应的力矩作为前馈力矩，并将脉冲指令和前馈力矩用于机器人的控制；
迭代学习模块用于补偿动力学模型误差，提高动力学模型精度，从而获得最优解和提高跟踪性能，具体是利用轨迹运行后获得的反馈力矩和前馈力矩以及存储模块中的上一次迭代的力矩偏差和迭代学习补偿项输入迭代学习控制器来获得新的迭代学习补偿项，用于补偿关节空间动力学模型的建模误差，从而更新轨迹规划模块中的动力学模型，用于轨迹规划模块进行轨迹规划；
存储模块用于存储每次迭代的迭代数据，以用于下一次迭代；迭代数据包括前馈力矩、反馈力矩、力矩偏差和迭代学习补偿项。
如图1所示的一种结合迭代学习的时间最优轨迹规划方法，包括以下步骤：
S1.获取任务路径
如图2所示，通过机器人控制柜202上的示教器203获得的任务路径。在任务路径已知的情况下，用路径位移s(k)∈[0,1]，0表示路径起点时，路径位移在路径总位移中的占比为0，1表示路径终点时，路径位移在路径总位移中的占比为1，从起始点s(0)开始，沿着末端路径到第k个点的路径位移在路径总位移中的占比，从而可以表示离散点k在空间中的位置。
S2.路径离散化
设定离散点数N，从而将步骤S1获得的任务路径离散化为N个点。如图2所示的任务路径，s(1)＝0表示在第一个离散点即轨迹起点的路径位移为0，s(N)＝1表示在第N个离散点即轨迹终点的路径位移为1,
Figure BDA0002041011900000111
表示在路径中间的离散点k的路径位移为
Figure BDA0002041011900000112
即从路径起点开始，沿着末端路径到第k个点的路径位移
Figure BDA0002041011900000113
在路径总位移
Figure BDA0002041011900000114
中的占比。
步骤S1和步骤S2在路径离散模块中实现，如图3所示，路径离散模块是本发明的轨迹规划控制器的一个模块，写入在机器人示教器203的外部存储设备301的中，通过示教器203中的示教板303规划机器人的初始任务路径，本实施例的外部存储设备301为硬盘。
S3.建立离散点的关节空间的约束不等式方程组
关节空间动力学模型的基础形式如下：
Figure BDA0002041011900000115
其中，τ表示n×1阶关节力矩矢量，即[τ1τ2…τn]T,其中n表示机器人关节数，τn表示第n个关节的力矩矢量，上标T表示转置，M(q)表示n×n阶正定质量矩阵；q表示n×1阶关节角度矢量，即[q1q2…qn]，qn表示第n个关节的矢量；
Figure BDA0002041011900000116
表示n×1阶关节加速度矢量，即
Figure BDA0002041011900000121
Figure BDA0002041011900000122
表示第n个关节的加速度矢量；B(q)表示n×n(n-1)/2阶的哥氏力系数矩阵，
Figure BDA0002041011900000123
表示n(n-1)/2×1阶的关节速度积矢量，即
Figure BDA0002041011900000124
Figure BDA0002041011900000125
表示n×1阶关节速度矢量，即
Figure BDA0002041011900000126
Figure BDA0002041011900000127
表示第n个关节的速度矢量；C(q)是n×n阶离心力系数矩阵，而
Figure BDA0002041011900000128
是n×1阶矢量，即
Figure BDA0002041011900000129
Fv表示n×1阶粘滞摩擦力向量；Fc表示n×1阶库伦摩擦力向量；G(q)表示重力向量；δl(q)是n×1阶迭代学习补偿向量，即[δ1,l(q1)δ2,l(q2)…δn,l(qn)]T,l为迭代次数，在没有迭代即l＝0时δl(q)设置为n×1阶0向量；sign表示符号函数；
对于离散点k，根据式(1)建立该点处的关节空间动力学模型：
Figure BDA00020410119000001210
其中，式(2)是式(1)关节空间动力学模型的离散形式，表示在第k个离散点的关节空间动力学模型，其中，k表示离散点，N表示总的离散点数，τ(k)表示n×1阶关节力矩矢量，即[τ1(k)τ2(k)…τn(k)]T,其中n表示机器人关节数，τn(k)表示第n个关节的力矩矢量，上标T表示转置，M(q(k))表示n×n阶正定质量矩阵；q(k)表示n×1阶关节角度矢量，即[q1(k)q2(k)…qn(k)]，qn(k)表示第n个关节的矢量；
Figure BDA00020410119000001211
表示n×1阶关节加速度矢量，即
Figure BDA00020410119000001212
Figure BDA00020410119000001213
表示第n个关节的加速度矢量；B(q(k))表示n×n(n-1)/2阶的哥氏力系数矩阵，
Figure BDA00020410119000001214
表示n(n-1)/2×1阶的关节速度积矢量，即
Figure BDA00020410119000001215
Figure BDA00020410119000001216
Figure BDA00020410119000001217
表示n×1阶关节速度矢量，即
Figure BDA00020410119000001218
Figure BDA00020410119000001219
表示第n个关节的速度矢量；C(q(k))是n×n阶离心力系数矩阵，而
Figure BDA00020410119000001220
是n×1阶矢量，即
Figure BDA00020410119000001221
Fv表示粘滞摩擦力向量；Fc表示库伦摩擦力向量；G(q(k))表示重力向量；δl(q(k))是n×1阶迭代学习补偿向量，即[δ1,l(q1(k))δ2,l(q(k))…δn,l(q(k))]T,l为迭代次数，在没有迭代即l＝0时δl(q(k))设置为n×1阶0向量；sign表示符号函数；
设定动力学约束条件为：
Tmin(k)≤τ(k)≤τmax(k)
其中，τmin(k)表示离散点k的最小约束力矩，τmax(k)表示离散点k的最大约束力矩；
设定运动学约束条件为
Figure BDA0002041011900000131
Figure BDA0002041011900000132
其中，
Figure BDA0002041011900000133
表示离散点k的最小约束速度，
Figure BDA0002041011900000134
表示离散点k的最大约束速度，
Figure BDA0002041011900000135
表示离散点k的最小约束加速度，
Figure BDA0002041011900000136
表示离散点k的最大约束加速度；
从而得到关节空间的约束不等式方程组
Figure BDA0002041011900000137
S4.转换为路径空间非线性优化模型
对于式(3)，根据链式规则，可将离散点k的速度
Figure BDA0002041011900000138
加速度
Figure BDA0002041011900000139
分解为路径空间中路径速度和路径加速度的方程
Figure BDA00020410119000001310
Figure BDA00020410119000001311
上式中，为了与关节空间区分开来，将路径空间的关节角度q(s(k))表示，其与q(k)是等价的，也表示在路径位移为s(k)时，路径函数q(s)的值；关节速度用
Figure BDA00020410119000001312
表示，其与
Figure BDA00020410119000001313
是等价的；将关节加速度用
Figure BDA00020410119000001314
表示，其与
Figure BDA00020410119000001315
是等价的；
Figure BDA00020410119000001316
是路径速度，是路径位移s(k)对时间t的一阶导数；
Figure BDA00020410119000001317
是路径加速度，是路径位移s(k)对时间t的二阶导数；q′(s(k))是关节角度q(s(k))对路径位移的一阶导，即路径位移为s(k)时，路径函数q(s)对路径位移的一阶微分函数q′(s)的值；
Figure BDA0002041011900000141
是关节角度q(S(k))对路径位移的二阶导，即路径位移为s(k)时，路径函数q(s)对路径位移的二阶微分函数q″(s)的值。
将式(4)、(5)代入式(2)、(3)得到路径空间的约束不等式方程组
Figure BDA0002041011900000142
其中
Figure BDA0002041011900000143
上述式(7)为关节空间动力学模型，是将式(4)和(5)代入关节空间动力学模型(2)得到的；
将式(7)简化为如下的路径空间动力学模型为：
Figure BDA0002041011900000144
其中
m(s(k))＝M(q(s(k)))q′(s(k))
c(s(k))＝M(q(s(k)))q″(s(k))+B(q(s(k))，q′(s(k)))q′(s(k))+C(q(s(k)))(q′(s(k)))2
f(s(k))＝Fv(q(s(k)))q′(s(k))
g(s(k))＝G(q(s(k)))
δl(s(k))＝δl(q(k))
使用路径位移s、路径速度
Figure BDA0002041011900000151
路径加速度
Figure BDA0002041011900000152
表示路径空间动力学模型(8)的基础形式：
Figure BDA0002041011900000153
式(9)表示路径位移为s、路径速度为
Figure BDA0002041011900000154
路径加速度为
Figure BDA0002041011900000155
与各关节力矩τ(s)的关系。
由于时间最优控制优化目标为使时间最小化，即
min T
由于
Figure BDA0002041011900000156
为了计算方便，将优化目标用下式表示
Figure BDA0002041011900000157
构建非线性优化模型为
Figure BDA0002041011900000158
Figure BDA0002041011900000159
其中s.t.表示约束条件符号，s(1)＝0表示路径起点离散点1的路径位移为0，s(N)＝1表示路径终点离散点N的路径位移为1，
Figure BDA00020410119000001510
表示路径起点离散点1的路径速度为0，
Figure BDA00020410119000001511
表示路径终点离散点N的路径速度为0。
S4.规划时间最优轨迹
根据步骤S3获得的非线性优化模型，在本实施例中，通过python非线性求解工具CasADi，求解得到时间最优轨迹即最优的
Figure BDA0002041011900000161
序列，其表示使时间最优时机器人在第k个点应该达到的路径速度。
S5.获得最优位移-时间函数
根据步骤S4获得的最优序列
Figure BDA0002041011900000162
由于
Figure BDA0002041011900000163
Figure BDA0002041011900000164
可得到路径速度sK和时间tK的关系为
Figure BDA0002041011900000165
其中K路径中的第K个离散点，t(K)表示从轨迹起点开始到第K个离散点所需要的时间。
通过分段最小二乘法拟合每个离散点的时间t(k)以及每个离散点处的路径位移s(k)的关系，得到连续的最优位移-时间函数s(t)。
拟合步骤如下：
S51.设置精度ε＝0.00001；
S52.选取三次多项式进行拟合s(t)＝a0+a1t+a2t2，式中a0、a1、a2为待求解的多项式系数，s(t)为要拟合的最优位移-时间函数s(t)；
S53.从路径离散点1开始，沿着路径位移增大方向，直到路径离散点z,参数
Figure BDA0002041011900000166
式中矩阵
S＝[s(1)s(2)…s(z)],
Figure BDA0002041011900000167
a(1)表示离散点1的路径位移；s(2)表示离散点2的路径位移；s(z)表示离散点n的路径位移，t(1)表示从轨迹起点开始到第1个离散点所需要的时间，t(2)表示从轨迹起点开始到第2个离散点所需要的时间，t(z)表示从轨迹起点开始到第z个离散点所需要的时间，最小二乘系数向量A＝[a0,a1,a2]T，其中上标T表示转置，A＝B\S，\表示矩阵右除，mean()是求解矩阵里面的平均值的函数,W是用来判断拟合路径函数误差是否符合精度要求的参数；
S54.令S＝[s(1)s(2)…s(z-1)],
Figure BDA0002041011900000171
A＝B\S＝[a0，a1,a2]T，求得的a0,a1,a2作为拟合函数拟合q1(s)＝a0+a1s+a2s2的系数，并且拟合函数为区间[t(1),t(z)]的最优位移-时间函数；
S55.根据步骤S51-S54，求解区间[0,1]内除了区间[t(1),t(z)]的其他区间的最优位移-时间函数，最终得到最优位移-时间函数。
S6.获得每个控制周期控制指令和前馈力矩
根据步骤S5获得的最优位移-时间函数，代入控制周期T控制周期到最优位移时间函数得到每个控制周期的最优位移s(T控制周期)，从而代入路径函数q(s)得到每个控制周期各关节的角度q(s(T控制周期))，根据电机每圈脉冲数P以及减速器传动比ρ得到每个控制周期的关节脉冲：
Pulse＝P×ρ×q(s(T控制周期))/2π
得到的每个控制周期的关节脉冲即为控制脉冲指令。
对步骤S5得到的最优位移-时间函数s(t)对时间t求一阶导得到最优速度-时间函数
Figure BDA0002041011900000172
将最优位移-时间函数s(t)对时间t求二阶导得到最优加速度-时间函数
Figure BDA0002041011900000173
将s(t)，
Figure BDA0002041011900000174
代入式(9)得最优轨迹的力矩-时间函数τ(s(t))，将控制周期T控制周期代入τ(s(t))得到每个控制周期的力矩，即为前馈力矩τ前馈(T控制周期)。将式(10)每个离散点k所需要的时间t(k)代入τ(s(t))得到每个离散点k的离散前馈力矩τ前馈(k)＝[τ1,前馈(k),τ2,前馈(k)…τn,前馈(k)]T，其中下标1，2，n表示关节数。将每个控制周期前馈力矩τ前馈(T控制周期)下发到控制柜，将每个离散点k的离散前馈力矩τ前馈(k)通过图3中的存储模块保存在外部存储设备301中。
S7.下发到机器人控制柜202运行
将步骤S6获得的控制指令Pulse和前馈力矩τ前馈(T控制周期)，下发到图3机器人控制柜202的伺服驱动器304中，驱动关节机器人201的伺服电动机305运行。
步骤S3-步骤S7在路径规划模块中实现，是图3轨迹规划控制器的一个模块，写入在示教器203的只读存储器302中。
S8.获得反馈测量力矩
通过下发伺服驱动器304的控制指令驱动关节机器人201的伺服电动机305运行，同时将伺服电动机305在运行过程中的实时测量力矩作为反馈力矩传回到示教器203中，通过类似步骤S51-S55的分段最小二乘拟合的方式得到反馈力矩τ反馈和时间t的关系τ反馈(t)，将式(10)每个离散点k所需要的时间t(k)代入代入τ反馈(t)，得到每个离散点的反馈力矩τ反馈(k)＝[τ1,反馈(k),τ2,反馈(k)…τn,反馈(k)]T，其中下标1，2，n表示关节数，通过调用在只读存储器302的存储模块将τ反馈(k)存储在外部存储设备301中。
S9.通过迭代学习控制器获得迭代学习补偿项
将存储在外部存储设备301中的前馈力矩τi,前馈(s(k))和反馈力矩τi,反馈(s(k))以及上一次迭代的迭代学习补偿项δi,l-1(s(k))输入到迭代学习控制器中，其中τi,反馈(s(k))表示第k个离散点第i个机器人关节的反馈力矩，τi,前馈(s(k))表示第k个离散点第i个机器人关节的前馈力矩，并且τi,反馈(s(k))和步骤S8的τ反馈(k)是等价的，τi,前馈(s(k))和步骤S6的τ前馈(k)是等价的。δi,l-1(s(k))表示第k个离散点第i个机器人关节第l-1次迭代的迭代学习补偿项。
在迭代学习控制器中，首先计算力矩偏差
Δτi,l(s(k))＝τi,反馈(s(k))-τi,前馈(s(k))
其中，Δτi,l(s(k))表示第k个离散点第i个机器人关节第l次迭代的力矩偏差。
然后计算收敛因子
Figure BDA0002041011900000191
其中γi,l表示第第i个机器人关节第l次迭代的收敛因子，Δτi,l-1(s(k))表示第k个离散点第i个机器人关节第l-1次迭代的力矩偏差，Δτi,l(s(k))表示第k个离散点第i个机器人关节第l次迭代的力矩偏差。
Figure BDA0002041011900000192
表示欧几里得范数的平方。
Figure BDA0002041011900000193
表示从括号中的数从1到N进行求和。
设l′是有效迭代次数，表示当力矩偏差成功减小时的迭代次数，根据γi,l的计算结果，迭代规则分为以下两种情况：
Case1.如果γi,l≥0，令l′＝l，γi,l保持不变；
Case2.如果γi,l<0,令γi,l＝0.5γi,l′，γi,l′＝0.5γi,l′，Δτi,l(s(k))＝Δτi,l′(s(k)),δi,l-1(s(k))＝δi,l′-1(s(k))，其中，γi,l′表示第l′次迭代，第i关节的收敛因子，Δτi,l′(s(k))表示第l′次迭代，第i关节，第k个离散点的力矩偏差，δi,l′-1(s(k))表示第l′-1次迭代，第i关节，第k个离散点的迭代学习补偿项。
最后将根据迭代规则更新后的收敛因子γi,l以及力矩偏差代入到迭代学习算法中
δi,l(s(k))＝γi,lΔτi,l(s(k))+δi,l-1(s(k))
为防止变化过快，将δi,l(s(k))滤波平滑处理后，再添加到式(5)的动力学模型中进行规划。滤波处理如下：
Figure BDA0002041011900000194
其中a表示滤波步长。
S10.更新机器人的关节空间动力学模型
根据步骤S9的迭代学习控制器，获得迭代学习补偿项，用于更新式(5)的机器人的关节空间动力学模型，即将新获得的迭代学习补偿项替换式(9)中旧的迭代学习补偿项，重新进行规划，并将规划后获得的τ(s(k))通过存储模块存储在外部存储设备301中。
S11.判断补偿项是否更新
根据补偿项是否更新，即迭代学习补偿项的收敛因子γi,l是否等于0，作为迭代终止的标志即关节空间动力学模型不再更新；如果补偿项更新，则转到步骤S3重新进行循环步骤S3-步骤S11，如果补偿项不更新，则停止迭代。
步骤S9-步骤S11在迭代学习模块实现，是轨迹规划控制器的一个模块，嵌入在示教器203里面。
步骤S1-步骤S11示例性描述了本发明的一种结合迭代学习的时间最优轨迹规划控制器及方法的实施例。但是应当理解，本发明不限于所公开的示例性实施例。以下权利要求的范围被赋予最广泛的解释，从而涵盖所有这种修改以及等同的结构和功能。

Claims (10)

1.一种结合迭代学习的时间最优轨迹规划控制器，其特征在于，所述轨迹规划控制器包括路径离散模块、轨迹规划模块、迭代学习模块和存储模块；
路径离散模块用于将机器人的任务路径离散化；
轨迹规划模块用于获得时间最优轨迹具体是根据约束条件，建立每个离散点的关节空间动力学模型，并将关节空间动力学模型转化到路径空间动力学模型并构建非线性优化模型，通过非线性求解工具求得机器人的时间最优轨迹，从而求得最优路径-时间函数，将控制周期代入最优位移-时间函数得到每个控制周期的最优位移，从而得到每个控制周期各关节的角度，并根据电机每圈脉冲数以及减速器传动比将每个控制周期各关节角度并转化为每个控制周期的脉冲指令，同时计算相应的力矩作为前馈力矩，并将脉冲指令和前馈力矩用于机器人的控制；
设定动力学约束条件为：
τmin(k)≤τ(k)≤τmax(k)
其中，τmin(k)表示离散点k的最小约束力矩，τmax(k)表示离散点k的最大约束力矩；τ(k)表示n×1阶关节力矩矢量；
设定运动学约束条件为
Figure FDA0003108407540000011
Figure FDA0003108407540000012
其中，
Figure FDA0003108407540000013
表示离散点k的最小约束速度，
Figure FDA0003108407540000014
表示离散点k的最大约束速度，
Figure FDA0003108407540000015
表示离散点k的最小约束加速度，
Figure FDA0003108407540000016
表示离散点k的最大约束加速度；
Figure FDA0003108407540000017
表示n×1阶关节加速度矢量，
Figure FDA0003108407540000018
表示关节的速度矢量；
从而得到关节空间的约束不等式方程组：
Figure FDA0003108407540000019
迭代学习模块用于补偿动力学模型误差，提高动力学模型精度，从而获得最优解和提高跟踪性能，具体是利用轨迹运行后获得的反馈力矩和前馈力矩以及存储模块中的上一次迭代的力矩偏差和迭代学习补偿项输入迭代学习控制器来获得新的迭代学习补偿项，用于补偿关节空间动力学模型的建模误差，从而更新轨迹规划模块中的动力学模型，用于轨迹规划模块进行轨迹规划；
存储模块用于存储每次迭代的迭代数据，以用于下一次迭代；迭代数据包括前馈力矩、反馈力矩、力矩偏差和迭代学习补偿项。
2.一种利用权利要求1所述轨迹规划控制器的结合迭代学习的时间最优轨迹规划方法，其特征在于，包括以下步骤：
S1.通过离线规划或利用示教器获得机器人的连续任务路径，并将连续任务路径输入到路径离散模块将路径离散化；
S2.将路径离散化后的结果输入到轨迹规划模块，获得时间最优轨迹，并转化为相应的脉冲指令，再将脉冲指令和前馈力矩下发到机器人控制柜；
S3.运行轨迹获得反馈力矩；
S4.计算前馈力矩和反馈力矩的力矩偏差，将计算结果和存储模块中的迭代数据输入到迭代学习模块中更新迭代学习补偿项，输出新的迭代学习补偿项；
S5.利用新的迭代学习补偿项更新迹规划模块中的关节空间动力学模型，用于轨迹规划模块中重新规划和运行；
S6.重复步骤S2-步骤S5直到关节空间动力学模型不再更新。
3.根据权利要求2所述的一种结合迭代学习的时间最优轨迹规划方法，其特征在于，步骤S1的路径离散模块将连续的任务路径离散为N个离散点，并假设相邻两点之间为匀加速运动。
4.根据权利要求2所述的一种结合迭代学习的时间最优轨迹规划方法,其特征在于，步骤S2具体包括以下步骤：
S21.建立每个离散点的机器人关节空间动力学模型，并根据动力学约束条件建立关节空间的力矩约束不等式方程组以及根据运动学约束条件建立速度约束不等式方程组和加速度约束不等式方程组；
S22.将步骤S21得到的关节空间的约束不等式方程组转化为路径空间的约束不等式方程组，并根据优化目标，构建非线性优化模型；
S23.根据非线性优化模型通过非线性求解工具求解机器人的时间最优轨迹；非线性求解工具包括python的CasADi、matlab的yamlip、sedumi和fmincon；
S24.根据时间最优轨迹得到最优位移-时间函数，将控制周期代入最优位移时间函数得到每个控制周期的最优位移，从而得到每个控制周期各关节角度，并根据电机每圈脉冲数以及减速器传动比将每个控制周期各关节角度转化为每个控制周期的脉冲指令，同时计算相应的力矩作为前馈力矩,用于后续控制。
5.根据权利要求4所述的一种结合迭代学习的时间最优轨迹规划方法，其特征在于，步骤S21具体包括如下：
建立如下的关节空间动力学模型的基础形式：
Figure FDA0003108407540000031
其中，τ表示n×1阶关节力矩矢量，即[τ1 τ2 … τn]T,其中n表示机器人关节数，τn表示第n个关节的力矩矢量，上标T表示转置，M(q)表示n×n阶正定质量矩阵；q表示n×1阶关节角度矢量，即[q1 q2 … qn]，qn表示第n个关节的矢量；
Figure FDA0003108407540000032
表示n×1阶关节加速度矢量，即
Figure FDA0003108407540000033
Figure FDA0003108407540000034
表示第n个关节的加速度矢量；B(q)表示n×n(n-1)/2阶的哥氏力系数矩阵，
Figure FDA0003108407540000035
表示n(n-1)/2×1阶的关节速度积矢量，即
Figure FDA0003108407540000036
Figure FDA0003108407540000037
表示n×1阶关节速度矢量，即
Figure FDA0003108407540000038
Figure FDA0003108407540000039
表示第n个关节的速度矢量；C(q)是n×n阶离心力系数矩阵，而
Figure FDA00031084075400000310
是n×1阶矢量，即
Figure FDA00031084075400000311
Fv表示n×1阶粘滞摩擦力向量；Fc表示n×1阶库伦摩擦力向量；G(q)表示重力向量；δl(q)是n×1阶迭代学习补偿向量，即[δ1,l(q1)δ2,l(q2)…δn,l(qn)]T,l为迭代次数，在没有迭代即l＝0时δl(q)设置为n×1阶0向量；sign表示符号函数；
根据式(1)建立离散点k的关节空间动力学模型：
Figure FDA0003108407540000041
其中，k表示离散点，N表示总的离散点数，τ(k)表示n×1阶关节力矩矢量，即[τ1(k)τ2(k)…τn(k)]T,其中n表示机器人关节数，τn(k)表示第n个关节的力矩矢量，上标T表示转置，M(q(k))表示n×n阶正定质量矩阵；q(k)表示n×1阶关节角度矢量，即[q1(k) q2(k) … qn(k)]，qn(k)表示第n个关节的矢量；
Figure FDA0003108407540000042
表示n×1阶关节加速度矢量，即
Figure FDA0003108407540000043
Figure FDA0003108407540000044
表示第n个关节的加速度矢量；B(q(k))表示n×n(n-1)/2阶的哥氏力系数矩阵，
Figure FDA0003108407540000045
表示n(n-1)/2×1阶的关节速度积矢量，即
Figure FDA0003108407540000046
Figure FDA0003108407540000047
Figure FDA0003108407540000048
表示n×1阶关节速度矢量，即
Figure FDA0003108407540000049
Figure FDA00031084075400000410
表示第n个关节的速度矢量；C(q(k))是n×n阶离心力系数矩阵，而
Figure FDA00031084075400000411
是n×1阶矢量，即
Figure FDA00031084075400000412
Fv表示粘滞摩擦力向量；Fc表示库伦摩擦力向量；G(q(k))表示重力向量；δl(q(k))是n×1阶迭代学习补偿向量，即[δ1,l(q1(k)) δ2,l(q(k)) … δn,l(q(k))]T,l为迭代次数，在没有迭代即l＝0时δl(q(k))设置为n×1阶0向量；sign表示符号函数；
设定动力学约束条件为：
τmin(k)≤τ(k)≤τmax(k)
其中，τmin(k)表示离散点k的最小约束力矩，τmax(k)表示离散点k的最大约束力矩；
设定运动学约束条件为：
Figure FDA00031084075400000413
Figure FDA0003108407540000051
其中，
Figure FDA0003108407540000052
表示离散点k的最小约束速度，
Figure FDA0003108407540000053
表示离散点k的最大约束速度，
Figure FDA0003108407540000054
表示离散点k的最小约束加速度，
Figure FDA0003108407540000055
表示离散点k的最大约束加速度；
从而得到关节空间的约束不等式方程组为：
Figure FDA0003108407540000056
6.根据权利要求4所述的一种结合迭代学习的时间最优轨迹规划方法，其特征在于，步骤S22具体包括如下：
根据链式规则，将离散点k的速度
Figure FDA0003108407540000057
和加速度
Figure FDA0003108407540000058
分解为如下的路径空间中路径速度和路径加速度的方程：
Figure FDA0003108407540000059
Figure FDA00031084075400000510
上式中，为了与关节空间区分开来，将路径空间的关节角度q(s(k))表示，其与q(k)是等价的，也表示在路径位移为s(k)时，路径函数q(s)的值；关节速度用
Figure FDA00031084075400000511
表示，其与
Figure FDA00031084075400000512
是等价的；将关节加速度用
Figure FDA00031084075400000513
表示，其与
Figure FDA00031084075400000514
是等价的；
Figure FDA00031084075400000515
是路径速度，是路径位移s(k)对时间t的一阶导数；
Figure FDA00031084075400000516
是路径加速度，是路径位移s(k)对时间t的二阶导数；q′(s(k))是关节角度q(s(k))对路径位移的一阶导，即路径位移为s(k)时，路径函数q(s)对路径位移的一阶微分函数q′(s)的值；
Figure FDA00031084075400000517
是关节角度q(s(k))对路径位移的二阶导，即路径位移为s(k)时，路径函数q(s)对路径位移的二阶微分函数q″(s)的值；
将式(4)、(5)代入式(2)、(3)得到路径空间的约束不等式方程组
Figure FDA0003108407540000061
其中
Figure FDA0003108407540000062
上述式(7)为在第k个离散点的路径空间动力学模型，是将式(4)和(5)代入关节空间动力学模型(2)得到的；将式(7)简化为如下的路径空间动力学模型：
Figure FDA0003108407540000063
其中
m(s(k))＝M(q(s(k)))q′(s(k))
c(s(k))＝M(q(s(k)))q″(s(k))+B(q(s(k)),q′(s(k)))q′(s(k))+C(q(s(k)))(q′(s(k)))2
f(s(k))＝Fv(q(s(k)))q′(s(k))
g(s(k))＝G(q(s(k)))
δl(s(k))＝δl(q(k))
上式中M(q(s(k)))、B(q(s(k)),q′(s(k)))、C(q(s(k)))、Fv(q(s(k)))、Fc(q(s(k)))、G(q(s(k)))是用来简化动力学方程的中间变量
所述优化目标为
min T
其中T表示运行任务路径所需要的时间，由于
Figure FDA0003108407540000064
Figure FDA0003108407540000071
为了计算方便，将优化目标用下式表示
Figure FDA0003108407540000072
所述非线性模型为
Figure FDA0003108407540000073
Figure FDA0003108407540000074
其中s.t.表示的是约束条件符号，s(1)＝0表示路径起点离散点1的路径位移为0，s(N)＝1表示路径终点离散点N的路径位移为1，
Figure FDA0003108407540000075
表示路径起点离散点1的路径速度为0，
Figure FDA0003108407540000076
表示路径终点离散点N的路径速度为0，δl(s(k)表示路径位移第l次迭代的迭代学习补偿项，N表示轨迹离散化后的离散点总数。
7.根据权利要求4所述的一种结合迭代学习的时间最优轨迹规划方法，其特征在于，步骤S24所述的最优路径-时间函数通过以下计算得到：
根据
Figure FDA0003108407540000077
的转换关系，得到到达每个离散点K所需要的运行时间：
Figure FDA0003108407540000078
其中k表示路径中的第k个离散点，t(k)表示从轨迹起点开始到第k个离散点所需要的时间，T表示轨迹的总运行时间；
通过分段最小二乘法拟合每个离散点的时间t(k)以及每个离散点处的路径位移s(k)的关系，得到连续的最优位移-时间函数s(t)；
所述前馈力矩的计算方法为：
将最优位移-时间函数s(t)对时间t求一阶导得到最优速度-时间函数
Figure FDA0003108407540000081
将最优位移-时间函数s(t)对时间t求二阶导得到最优加速度-时间函数
Figure FDA0003108407540000082
将s(t)，
Figure FDA0003108407540000083
代入式(6)得最优轨迹的力矩-时间函数τ(s(t))，将控制周期T控制周期代入τ(s(t))得到每个控制周期的力矩，即为前馈力矩。
8.根据权利要求2所述的一种结合迭代学习的时间最优轨迹规划方法，其特征在于，步骤S4所述包括以下步骤：
S41.将获得的测量力矩离散化；
S42.计算前馈力矩和反馈力矩的力矩偏差，将获得的力矩偏差和存储模块中上一次迭代的力矩偏差以及迭代学习补偿项输入到迭代学习模块的迭代学习控制器中，更新迭代学习补偿项；
S43.将更新后的迭代学习补偿项平滑滤波处理后再添加到轨迹规划模块中，用于更新轨迹规划模块中的式(2)的关节空间动力学模型，重新轨迹进行规划。
9.根据权利要求8所述的一种结合迭代学习的时间最优轨迹规划方法，其特征在于，迭代学习控制器中的迭代学习算法如下：
δi,l(s(k))＝γi,lΔτi,l(s(k))+δi,l-1(s(k)) (6)
其中i＝1…n表示机器人的关节编号，Δτi,l(s(k))＝τi,反馈(s(k))-τi,前馈(s(k))表示力矩偏差，其中τi,反馈(s(k))表示第k个离散点第i个机器人关节的反馈力矩，τi,前馈(s(k))是第k个离散点第i个机器人关节的前馈力矩；l表示迭代次数，γi,l是第i个机器人关节第l次迭代的收敛因子，用于控制迭代学习的收敛，δi,l-1(s(k))是第k个离散点第i个机器人关节第l-1次迭代的迭代学习补偿项，用于保存此前的学习行为；
其中，为了使迭代收敛，收敛因子γi,l的计算公式如下：
Figure FDA0003108407540000091
其中γi,l表示第i个机器人关节第l次迭代的收敛因子，Δτi,l-1(s(k))表示第k个离散点第i个机器人关节第l-1次迭代的力矩偏差，Δτi,l(s(k))表示第k个离散点第i个机器人关节第l次迭代的力矩偏差，
Figure FDA0003108407540000092
表示欧几里得范数的平方，
Figure FDA0003108407540000093
表示从括号中的数从1到N进行求和；
设l′是有效迭代次数，表示当力矩偏差成功减小时的迭代次数，根据γi,l的计算结果，迭代规则分为以下两种情况：
Case1.如果γi,l≥0，令l′＝l，返回式(6)进行迭代；收敛因子γi,l等于0，作为迭代终止的标志即关节空间动力学模型不再更新；
Case2.如果γi,l<0,令γi,l＝0.5γi,l′，γi,l′＝0.5γi,l′，Δτi,l(s(k))＝Δτi,l′(s(k)),δi,l-1(s(k))＝δi,l′-1(s(k))，返回式(6)进行迭代，其中，γi,l′表示第l′次迭代，第i关节的收敛因子，Δτi,l′(s(k))表示第l′次迭代，第i关节，第k个离散点的力矩偏差，δi,l′-1(s(k))表示第l′-1次迭代，第i关节，第k个离散点的迭代学习补偿项。
10.根据权利要求8所述的一种结合迭代学习的时间最优轨迹规划方法，其特征在于，步骤S43所述的平滑滤波处理如下：
Figure FDA0003108407540000094
其中a表示滤波步长。
CN201910341967.9A 2019-04-26 2019-04-26 一种结合迭代学习的时间最优轨迹规划控制器及方法 Expired - Fee Related CN110221538B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910341967.9A CN110221538B (zh) 2019-04-26 2019-04-26 一种结合迭代学习的时间最优轨迹规划控制器及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910341967.9A CN110221538B (zh) 2019-04-26 2019-04-26 一种结合迭代学习的时间最优轨迹规划控制器及方法

Publications (2)

Publication Number Publication Date
CN110221538A CN110221538A (zh) 2019-09-10
CN110221538B true CN110221538B (zh) 2021-10-01

Family

ID=67819969

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910341967.9A Expired - Fee Related CN110221538B (zh) 2019-04-26 2019-04-26 一种结合迭代学习的时间最优轨迹规划控制器及方法

Country Status (1)

Country Link
CN (1) CN110221538B (zh)

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110703693B (zh) * 2019-10-15 2020-11-17 华中科技大学 一种机床进给系统的迭代学习前馈控制方法及系统
CN111185923B (zh) * 2020-01-14 2022-02-15 深圳众为兴技术股份有限公司 一种机器人控制装置及控制方法
CN111399514B (zh) * 2020-03-30 2023-05-30 浙江钱江机器人有限公司 一种机器人时间最优轨迹规划方法
CN111551178B (zh) * 2020-04-27 2022-02-18 广东工业大学 一种基于最短路径的分段轨迹时间规划方法
CN111983924B (zh) * 2020-07-29 2022-03-15 华南理工大学 基于强化学习的机器人时间最优轨迹规划方法及控制器
CN114055459B (zh) * 2020-08-06 2023-05-02 库卡机器人(广东)有限公司 轨迹规划的方法、装置、电子设备及存储介质
CN112318505B (zh) * 2020-10-28 2021-11-16 江南大学 一种移动机器人变批次长度迭代学习优化控制方法
CN113848809B (zh) * 2021-09-24 2023-07-18 中建三局集团有限公司 基于数字滤波的轨迹规划方法
CN115870974B (zh) * 2022-11-15 2024-05-17 华南理工大学 基于余弦相似度比较的多轴运动系统时间最优轨迹规划方法
CN117148731B (zh) * 2023-11-01 2024-01-09 佛山科学技术学院 基于单点误差提取的可变迭代运动控制方法及装置

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6216058B1 (en) * 1999-05-28 2001-04-10 Brooks Automation, Inc. System of trajectory planning for robotic manipulators based on pre-defined time-optimum trajectory shapes
JP3677623B2 (ja) * 2000-09-11 2005-08-03 独立行政法人産業技術総合研究所 脚式ロボットのリアルタイム最適制御方法
CN102554938B (zh) * 2010-12-31 2014-12-03 北京中科广视科技有限公司 机器人的机械手末端轨迹跟踪方法
CN105773623B (zh) * 2016-04-29 2018-06-29 江南大学 基于预测型间接迭代学习的scara机器人轨迹跟踪控制方法
CN108319144B (zh) * 2018-02-21 2021-07-09 湘潭大学 一种机器人轨迹跟踪控制方法及系统

Also Published As

Publication number Publication date
CN110221538A (zh) 2019-09-10

Similar Documents

Publication Publication Date Title
CN110221538B (zh) 一种结合迭代学习的时间最优轨迹规划控制器及方法
Carron et al. Data-driven model predictive control for trajectory tracking with a robotic arm
CN108319144B (zh) 一种机器人轨迹跟踪控制方法及系统
CN105773623B (zh) 基于预测型间接迭代学习的scara机器人轨迹跟踪控制方法
CN109202894B (zh) 进行学习控制的机器人及其控制方法
Aangenent et al. Data-based optimal control
CN114761966A (zh) 用于以轨迹为中心的基于模型的强化学习的稳健优化的系统及方法
US10152033B2 (en) Proportional integral derivative control incorporating multiple actuators
CN112171677B (zh) 基于lstm迟滞模型的机器人柔性关节补偿控制方法
CN115256395A (zh) 基于控制障碍函数的模型不确定机器人安全控制方法
Kramberger et al. Transfer of contact skills to new environmental conditions
CN113199477B (zh) 一种基于强化学习的Baxter机械臂轨迹跟踪控制方法
Verdonck et al. Improving the dynamic accuracy of industrial robots by trajectory pre-compensation
CN116460860A (zh) 一种基于模型的机器人离线强化学习控制方法
CN114995137B (zh) 基于深度强化学习的绳驱并联机器人控制方法
Minh Nguyet et al. A neural flexible PID controller for task-space control of robotic manipulators
Maeda et al. Improving operational space control of heavy manipulators via open-loop compensation
US11673264B2 (en) System and method for robotic assembly based on adaptive compliance
US20220308530A1 (en) System for Performing a Task According to a Reference Trajectory
Ganin et al. Hierarchical adaptive control system of a manipulator based on the synthesis of a neural network of fuzzy inference and an iterative refinement algorithm
Koubaa et al. Intelligent control for nonholonomic mobile robot including actuator dynamics
WO2022201377A1 (ja) ロボット制御装置およびロボット制御方法
Argüz et al. Experimental evaluation of the success of peg-in-hole tasks learned from demonstration
Korayem et al. Determining maximum load-carrying capacity of robots using adaptive robust neural controller
Wahrburg et al. Extending dynamic movement primitives towards high-performance robot motion

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
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20211001

CF01 Termination of patent right due to non-payment of annual fee