一种基于柔性关节模型的时间最优轨迹规划方法

1.本发明属于机器人轨迹规划领域，具体涉及一种基于柔性关节模型的时间最优轨迹规划方法，其建立一个新的柔性关节动力学模型，该模型考虑机器人关节中柔性因素对运动的影响，更加符合轻型机器人的实际运动规律。同时使用序列二次规划(sequential quadratic programming，sqp)方法来寻找机器人时间最优轨迹。


背景技术：

2.随着科技的发展和制造业的进步，需要机器人和人类协作的场合越来越多，人与机器人的直接接触越来越频繁，而原本的工业机器人控制简单、功能单一，并且笨重危险，显然不能胜任这一工作，轻型机器人也随之应运而生。
3.相较于工业机器人，轻型机器人有很多改进，但这放大了机器人关节中柔性因素对运动的影响。首先轻型机器人需要灵巧地工作，以胜任人手的功能，这需要多种传感器加以支持，其中包括力矩传感器，力矩传感器原理是通过应变梁的形变检测扭矩；同时轻型机器人大多采用谐波减速器，该减速器带有柔轮。这些都增加关节的柔性。此外，柔性也增加了机器人发生碰撞时的缓冲，起到了保护人和机器人的作用。基于上述原因，轻型机器人在运动时电机角位移和其驱动的连杆的角位移并不相同，所以在建立机器人动力学模型时，将因力矩传感器和谐波减速器产生的弹性形变抽象为一个弹簧，两侧连接机器人连杆和电机。
4.然而当下在设计机器人最优轨迹规划时，依然使用把机器人视为纯刚体结构的动力学模型，这样得到的轨迹不仅不是机器人的最优轨迹，并且机器人末端执行器在运动过程中会因柔性关节的影响而产生振动，使得轨迹不光滑并且可能导致轨迹变形。


技术实现要素：

5.本发明的目的是提出一种新的柔性关节动力学模型；并以时间最优为指标要求，提出一种基于sqp算法的关节空间轨迹规划方法，计算出时间最优位置点，再通过五次b样条插值方法得到机器人时间-位置控制序列，该序列让机器人运行时间更短、速度曲线平滑、振动更小。
6.一种基于柔性关节模型的时间最优轨迹规划方法，包括以下步骤：
7.步骤1：将机器人末端执行器的目标路径均分为多份，最终得到一个路径端点序列a＝[a1，a2，...，an]
t
；ai为第i个路径端点的坐标；i＝1，2，...，n。n为路径端点数量。
[0008]
步骤2：根据路径端点序列a获得关节路径序列b＝[b1，b2，...，bn]
t
，其中bi＝[q
i，1
，q
i，2
...，q
i，n
]
t
，q
i，j
为第i个路径端点对应的第j个关节的角位移。j＝1，2，...，n。n为机器人的关节数。
[0009]
步骤3：构建约束关系。
[0010]
3-1.使用柔性关节动力学模型描述机器人的每个关节；柔性关节动力学模型中，伺服电机(1)的输出轴与连杆(3)之间通过柔性关节(2)连接；柔性关节(2)使得伺服电机
(1)的输出轴与连杆(3)之间能够发生弹性转动。
[0011]
基于柔性关节动力学模型构建等式约束如下：
[0012][0013]
其中，m
l
为连杆质量矩阵；mm为电机转子质量矩阵；θ为电机转动角位移；q为连杆角位移；k(θ-q)为柔性关节的扭矩；为科里奥利力；g(q)为重力项；τ
ext
为外力对各关节的力矩；τm为电机输出力矩；是机器人运动开始时的连杆角位移，是机器人运动结束后的连杆角位移。t为路径运行总时间。
[0014]
3-2.构建不等式约束如下：
[0015][0016]
其中，τ
m_min
、τ
m_max
分别为电机输出力矩的下限、上限；q
minqmax
分别为连杆角位移的下限、上限；分别为电机角速度的下限、上限；τ
e_min
、τ
e_max
分别为柔性扭矩的下限、上限。
[0017]
步骤4：根据步骤3得到的等式约束和不等式约束构建优化问题，并求出最优解，时间最优轨迹序列c＝[c1，c2，...，cn]
t
。
[0018]
步骤5：对步骤4得到的时间最优轨迹序列c进行插补，得到机器人控制序列d。
[0019]
步骤6：机器人的各关节根据机器人控制序列d进行控制，使得机器人的末端执行器沿目标轨迹运动。
[0020]
作为优选，步骤2中，获取获得关节路径序列b的过程为：建立机器人的dh参数表，再通过机器人逆运动学，将机器人末端执行器的笛卡尔空间坐标转换为关节空间坐标，从而将路径端点序列a转换为关节路径序列b。
[0021]
作为优选，步骤2执行后，将必要数据发送给上位机。必要数据包括关节路径序列b、各关节的电机扭矩的上下限、连杆转动角度的上下限、电机速度的上下限和柔性扭矩的上下限。
[0022]
作为优选，步骤4的具体过程为：构建优化问题如下：
[0023][0024][0025]
式中，l为等式约束的个数；m为不等式约束的个数；是梯度算子；hi(xk)为优化问题的第i个等式约束；gi(xk)为优化问题的第i个不等式约束。dk为第k次迭代的搜索方向。lk为第k次迭代的拉格朗日方程。对于给定的拉格朗日算子μ，λ，有：为第k次迭代的拉格朗日方程。对于给定的拉格朗日算子μ，λ，有：
[0026]
求解该优化问题的迭代步骤为：1)给定初始点x0，设置收敛精度ε，令l0＝i；i为单位矩阵；2)将原问题在迭代点xk处简化成二次规划问题；3)求解上述二次规划问题，求得搜索方向dk，得到下一个迭代点x
k+1
＝xk+dk；4)判断是否满足迭代终止条件，若满足条件则将x
k+1
作为最优解，f(x
k+1
)作为目标函数的最优值，终止计算；否则，进入步骤5)；5)计算新的拉格朗日函数l
k+1
，进入步骤2)。
[0027]
作为优选，步骤5中，利用k次b样条曲线对时间最优轨迹序列c进行插补；k的取值为3或5。插补的具体过程如下：
[0028]
k次b样条曲线的表达形式为：
[0029]
式中，di为曲线的控制顶点；n
i，k
(u)表示k次b样条的基函数：
[0030][0031]
式中，k表示b样条的阶数，i表示b样条曲线控制顶点的序号，n
i，k
(u)的区间为u∈[ui，u
i+k+1
]，其节点矢量u∈[u0，u1，...，u
n+2k
]。
[0032]
将b样条曲线的表达形式更新为：
[0033][0034]
采用累积弦长参数化法对u
k+1
，u
k+2
，...，u
n+k-1
归一化：
[0035][0036]
经过k次b样条插值后，得到机器人控制序列d＝[d1，d2，...，dn]
t
。
[0037]
本发明的有益效果为：
[0038]
本发明结合轻型机器人的关节处因同步带传动机构、谐波减速器而存在控制滞后的特点，构建机器人柔性关节的动力学模型，并基于该动力学模型进行各关节最优轨迹的求解，获得了轻型机器人的时间最优轨迹；此外，本发明采用的序列二次规划方法在求解非线性优化问题上表现优秀，提高了本技术获得的时间最优轨迹的准确性。
附图说明
[0039]
图1为本发明的轨迹规划流程图；
[0040]
图2为本发明中机器人单个关节的动力学模型的传动系统图；
[0041]
图3为本发明步骤4中迭代求解的流程图。
[0042]
附图标记：图2中，1为伺服电机，2为柔性关节，3为连杆。
具体实施方式
[0043]
为使本发明的目的、技术方案和优点更加清楚明白，下面结合具体实施例，并参照附图，对本发明作进一步详细说明。
[0044]
本实施例中，使用广州启帆工业机器人公司研发的小型负载关节机器人sr1-540作为被控制的机器人。sr1-540型机器人的组成部分主要包括机器人本体、控制系统和驱动系统。在一次执行命令中，需要机器人完成一条直线的轨迹。
[0045]
一种基于柔性关节模型的时间最优轨迹规划方法，包括以下步骤：
[0046]
步骤1：获取一条机器人末端执行器的目标路径。sqp优化算法适用于离散算法，故需将路径离散化，离散方法为将路径均分为多份，最终得到一个路径端点序列a＝[a1，a2，...，an]
t
，其中ai＝[xi，yi，zi]
t
，xi，yi，zi是在机器人基坐标系下的x轴、y轴、z轴坐标值。n为离散出的路径端点数量。该路径可以是通过示教模式手动移动机器人获得，也可以直接给出。本实施例中，n的数值为100；a1，a2，...，an通过对目标路径进行均分得到。
[0047]
步骤2：建立机器人的dh参数表，再通过机器人逆运动学，将机器人末端执行器笛卡尔空间坐标转换为关节空间坐标，从而将笛卡尔路径序列转换为关节空间序列。一个n自由度的机器人，可以获得在末端执行器各个路径端点的关节路径序列b＝[b1，b2，...，bn]
t
，其中bi＝[q
i，1
，q
i，2
...，q
i，n
]
t
，q
i，j
为第i个路径端点对应的第j个关节的角位移。j＝1，2，...，n；n为机器人的关节数；sr1-540型机器人为6自由度机器人，故本实施例中n＝6。
[0048]
sr1-540型机器人的dh参数表，如表1所示。
[0049]
表1
[0050][0051]
步骤3：将必要数据发送给上位机。必要数据包括关节路径序列b、各关节的电机扭矩的上下限、连杆转动角度的上下限、电机速度的上下限和柔性扭矩的上下限。在本实施例中，算法求解是通过电脑的matlab 2021b完成。在sr1-540型机器人的用户手册中，提供了机器人各项参数，包括连杆质量m＝[3.36，6.80，5.22，3.12，2.30，0.66]
t
，单位kg；连杆的惯量矩阵i＝[i1，i2，...，i6]
t
；连杆转角下限位q
min
＝[-145，-10，-34，-220，-120，-360]
t
，转角上限位q
max
＝[120，175，218，220，200，360]
t
，单位为度；电机的最大输出力矩τ
m_max
＝[85，195，80，50，56，17]
t
，单位为牛顿
·
米；电机的最大转速v
max
＝[125，160，384，125，251，480]
t
，单位为度每秒。
[0052]
根据以上参数建立一个时间最优问题。
[0053]
建立一个最优问题包含目标函数、等式约束和不等式约束这三个方面。本发明解决的是时间最优问题，故目标函数为其中t为路径运行总时间。
[0054]
1)等式约束。等式约束为机器人的动力学模型，本发明提供了一个柔性关节动力学模型，该模型考虑了机器人关节中柔性因素的影响。图2为一个柔性关节传动系统的模型。柔性关节传动系统伺服电机1的输出轴与连杆3之间通过柔性关节2连接；柔性关节2允许伺服电机1的输出轴与连杆3之间发生弹性转动，从而更贴合机器人关节的实际工作状态。
[0055]
设定相关的参数如下：mm为电机转子质量矩阵，θ为电机输出到柔性关节的角位移(即电机经减速器减速后的输出角位移)，k为关节刚度，m
l
为连杆质量矩阵，q为连杆角位移。连杆角位移q与电机输出到柔性关节的角位移θ的差值即为电机与连杆之间传动结构弹性变形带来的相对角位移；
[0056]
具体推导公式如下：
[0057]
利用euler-lagrange方程推导动力学方程，需要求出机器人系统的动势能表达式。
[0058]
连杆的动能表达式为：式中，为连杆角位移q的一阶导数(即角速度)。
[0059]
电机转子i的绝对速度为：式中，v
mi
为第i个关节的电机转子相对基坐标系的绝对速度，j
mi
(q)为从第i个关节到机器人基坐标系的速度雅克比矩阵，ti为第i个关节到机器人基坐标系的齐次变换矩阵，zi为第i个关节的电机转子的轴线在第i个关节坐标系下的齐次坐标。
[0060]
故电机转子的动能表达式为：式中，m
mi
为第i个关节的电机转子的质量矩阵。
[0061]
将上式写成矩阵形式表示为：式中，j为电机转子经减速后的转动质量矩阵；s(q)为耦合部分的质量矩阵，属于严格上三角矩阵且只与减速比和电机转子转动惯量有关。
[0062]
综上，柔性关节机器人完整的动能方程为：综上，柔性关节机器人完整的动能方程为：
[0063]
势能方程表示为：其中，k(θ-q)为柔性关节的扭矩。
[0064]
将上述动势能表达式代入euler-lagrange方程，整理后可得：
[0065]
其中g(q)为重力项，其中g(q)为重力项，其中g(q)为重力项，为科里奥利力，τ
ext
为外力对各关节的力矩，τm为电机输出力矩。
[0066]
由于完整模型中耦合项s(q)的存在，增加了动力学模型的复杂度。但在大多数情况下电机转子的转动惯量要远小于连杆的转动惯量，因此由于连杆运动所影响的那部分转子平动动能完全可以忽略。此时，令s(q)＝0。得到简化后的机器人动力学方程如下：
[0067][0068]
上式是优化问题的等式约束。此外，机器人在启动和停止时的速度为0，在停止时应到达指定位置，所以还有等式约束为：式中，t为轨迹运行的总时间，是机器人运动开始时的连杆角位移，是机器人运动结束后的连杆角位移。
[0069]
2)不等式约束。为使机器人能够快速平滑的运动，同时也保护电机避免过载运动，故本发明设计了电机扭矩约束、连杆转动角度约束、电机速度约束和柔性扭矩约束：其中，τ
m_min
、τ
m_max
分别为电机输出力矩的下限、上限；q
min
、q
max
分别为连杆角位移的下限、上限；分别为电机角速度的下限、上限；τ
e_min
、τ
e_max
分别为柔性扭矩的下限、上限。
[0070]
步骤4：求出问题的最优解。
[0071]
以连杆角位移q作为被优化对象，作为迭代点；对于一个优化问题：
[0072][0073]
利用泰勒展开将上述问题的目标函数在迭代点xk处简化成二次函数，将约束条件简化成线性函数后得到如下二次规划问题：
[0074][0075]
式中，l为等式约束的个数；本实施例中，l＝5；m为不等式约束的个数；本实施例中，m＝8。hi(xk)表示第i个等式约束；gi(xk)表示第i个不等式约束；dk为第k次迭代的搜索方向。lk为第k次迭代的拉格朗日方程。对于给定的拉格朗日算子μ，λ，有：为第k次迭代的拉格朗日方程。对于给定的拉格朗日算子μ，λ，有：
[0076]
求解此二次规划问题，可得原问题的下一个搜索方向dk，则有x
k+1
＝xk+dk。
[0077]
综上所述，序列二次规划算法的迭代步骤为：1)给定初始点x0，设置收敛精度ε，令l0＝i；i为单位矩阵；2)将原问题在迭代点xk处简化成二次规划问题；3)求解上述二次规划
问题，求得搜索方向dk，得到下一个迭代点x
k+1
；4)判断是否满足迭代终止条件，若满足条件则将x
k+1
作为最优解，f(x
k+1
)作为目标函数的最优值，终止计算；否则，进入步骤5)；5)计算新的拉格朗日函数l
k+1
，进入步骤2)。
[0078]
具体流程图见图3。最终得到时间最优轨迹序列c＝[c1，c2，...，cn]
t
。
[0079]
步骤5：利用五次b样条曲线对时间最优轨迹序列c进行插补。b样条计算效率高且平滑性足够好，又由于b样条曲线的局部可控性和凸包性质特点，可以进行轨迹优化。
[0080]
k次b样条曲线的表达形式为：
[0081]
式中，di(i＝0，1，...，n)为曲线的控制顶点，n
i，k
(u)，(i＝0，1，...，n)表示k次b样条的基函数：
[0082][0083]
式中，k表示b样条的阶数，i表示b样条曲线控制顶点的序号，n
i，k
(u)的区间为u∈[ui，u
i+k+1
]，其节点矢量u∈[u0，u1，...，u
n+2k
]。
[0084]
b样条曲线具有局部支撑性，对于u∈[ui，u
i+k+1
]，只有至多k+1个非零基函数n
r，k
(u)(r＝i-k，i-k+1，...，i)。因此b样条曲线也可表示为：
[0085][0086]
为了使k次b样条曲线经过起始点和终止点，需要满足条件u0＝u1＝...＝uk＝0，u
n+k
＝u
n+k+1
＝...＝u
n+2k
＝1。采用累积弦长参数化法对u
k+1
，u
k+2
，...，u
n+k-1
归一化：
[0087][0088]
经过五次b样条插值后，得到机器人控制序列d＝[d1，d2，...]
t
。机器人控制序列d中的控制节点个数为插值后的总节点数。
[0089]
步骤6：将控制序列d发送到机器人控制器中，控制机器人完成制定轨迹。