一种机器人关节加速度约束规划方法和系统与流程

1.本发明属于机器人运动轨迹运动学规划领域，更具体地，涉及一种机器人关节加速度约束规划方法和系统。


背景技术：

2.随着工业机器人技术的发展，其在制造业中的应用越来越广泛。针对简单的物体搬运任务，通常要求机器人在任务中的轨迹达到加速度连续，同时满足对应的加速度幅值约束。根据机器人运动学，其末端加速度又受到各个关节的电机轴的加速度幅值约束。在进行机器人轨迹的运动学规划时，通常给定关节的电机轴的加速度幅值约束为定值，从而去规划整个轨迹的运动学。但是，在实际的任务中，各个关节的电机轴的加减速能力受到不同位姿下各个关节的电机轴上的等效转动惯量和末端负载的影响。若保守的将每个关节的电机轴的加速度幅值约束作为定值，则在不同的位姿下不能完全发挥对应的各个关节的电机轴的加减速能力，从而影响机器人的运动效率。因此，研究考虑不同位姿下机器人的各个关节的电机轴等效转动惯量对加速度约束的影响，研究不同姿态下各关节的电机轴的最大加速度，充分发挥机器人的加减速能力，对于提高机器人的运动效率很有必要。


技术实现要素：

3.针对相关技术的缺陷，本发明的目的在于提供一种机器人关节加速度约束规划方法和系统，旨在解决机器人在运动过程中各个关节的电机轴的加减速能力没有得到充分利用的问题。
4.为实现上述目的，本发明的一个方面提供了一种机器人关节加速度约束规划方法，包括以下步骤：
5.建立机器人各连杆的d
‑
h模型，得到d
‑
h连杆坐标系的齐次变换矩阵和旋转变换矩阵；
6.根据所述齐次变换矩阵和旋转变换矩阵，分别将各个连杆的质心坐标从基坐标系变换到各自对应的d
‑
h连杆坐标系下，进而获取当前位姿下各个关节的电机轴上的等效转动惯量；
7.考虑负载的影响，更新各个关节的电机轴上的等效转动惯量；
8.建立各个连杆的动力学方程，得到加速度与电机输出转矩的函数关系；
9.根据各个关节的电机轴上的等效转动惯量和所述函数关系，得到加速度的幅值约束，从而规划机器人运动时各个关节的电机轴的加速度。
10.进一步地，所述d
‑
h连杆坐标系的齐次变换矩阵通过相邻d
‑
h连杆坐标系的齐次变换矩阵相乘得到：
11.[0012][0013]
其中，为绕轴z
i
旋转θ
i
的旋转变换，为沿着z
i
轴方向移动d
i
的平移变换，为绕轴x
i
旋转α
i
的旋转变换，为沿着x
i
轴方向移动a
i
的平移变换，为第i个d
‑
h连杆坐标系相对于第i
‑
1个d
‑
h连杆坐标系的旋转变换矩阵，为第i个d
‑
h连杆坐标系的原点在第i
‑
1个d
‑
h连杆坐标系中的坐标。
[0014]
进一步地，根据所述齐次变换矩阵和旋转变换矩阵，分别将各个连杆的质心坐标从基坐标系变换到各自对应的d
‑
h连杆坐标系下包括：
[0015][0016][0017][0018][0019][0020][0021]
其中θ
i
为第i个连杆的d
‑
h连杆坐标系z
i
轴的关节转角，i＝1，2，3，4，5，6，各个连杆在基坐标系下的质心坐标定义为0p
i
，各个连杆在对应d
‑
h连杆坐标系下的质心坐标定义为
i
p
i
。
[0022]
进一步地，所述当前位姿下各个关节的电机轴上的等效转动惯量为：
[0023][0024]
其中
i
x
j
，
i
y
j
为第j个连杆的质心在第i个d
‑
h连杆坐标系下的横纵坐标，为第j个连杆绕过其质心且与z
i
轴平行的轴旋转的转动惯量，m
j
为第j个连杆的质量。
[0025]
进一步地，考虑负载的影响，更新各个关节的电机轴上的等效转动惯量包括：
[0026]
将负载的重量折算到第六连杆上，
[0027]
m
′6＝m6 m
[0028]6x6′
＝(6x6·
m6 6x
·
m)/(m6 m)
[0029]6y6′
＝(6y6·
m6 6y
·
m)/(m6 m)
[0030]6z6′
＝(6z6·
m6 6z
·
m)/(m6 m)
[0031]
将得到的新的第六连杆质心的质量m6′
和坐标(6x6′
，6y6′
，6z6′
)代替原来第六连杆质心的质量m6和坐标(6x6，6y6，6z6)，重新计算各个关节的电机轴上的等效转动惯量。
[0032]
进一步地，所述加速度与电机输出转矩的函数关系为：
[0033][0034]
其中，m
i
为第i个关节电机的输出转矩，为该关节的电机轴的角加速度矢量，其方向沿着z
i
轴的轴向，m
f，i
为该关节的摩擦转矩，为关节的电机轴驱动的所有连杆的重力矩之和。
[0035]
本发明的另一方面提供了一种机器人关节加速度约束规划系统，包括：计算机可读存储介质和处理器；
[0036]
所述计算机可读存储介质用于存储可执行指令；
[0037]
所述处理器用于读取所述计算机可读存储介质中存储的可执行指令，执行上述的机器人关节加速度约束规划方法。
[0038]
通过本发明所构思的以上技术方案，与现有技术相比，本发明针对机器人在运动过程中的加速度约束，使得机器人在不同位姿下，能综合考虑机器人的连杆重力和末端负载对各个转动关节的电机轴的等效转动惯量影响，分析该姿态下关节的电机轴加速度的最值，从而分析规划运动过程中加速度的约束，提高了机器人的运动效率。
附图说明
[0039]
图1是本发明实施例加速度规划流程图。
具体实施方式
[0040]
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
[0041]
为实现上述目的，本发明实施例提供了一种根据机器人不同位姿下各个关节电机轴的等效转动惯量变化的关节加速度约束规划方法，包括下列步骤：
[0042]
(a)根据机器人各个连杆的参数，建立对应的d
‑
h(denavit
‑
hartenberg)模型。建立d
‑
h连杆坐标系的原则为：
[0043]
1.z
i
轴沿关节i 1的轴向。
[0044]
2.原点o
i
为z
i
‑1和z
i
轴的交点或其公垂线与关节的电机轴z
i
的交点。
[0045]
3.x
i
轴沿z
i
‑1和z
i
轴的公垂线方向。
[0046]
4.y
i
轴按照右手定则确定。
[0047]
每个参数的含义：
[0048]
a
i
：连杆长度a
i
定义为从z
i
‑1移动到z
i
的距离，沿x
i
轴指向为正。其实质为公垂线的
长度。
[0049]
α
i
：连杆扭角α
i
定义为z
i
‑1旋转到z
i
的角度，绕x
i
轴正向旋转为正。
[0050]
d
i
：关节偏移量d
i
定义为x
i
‑1移动到x
i
的距离，沿z
i
轴指向为正。其实质为两条公垂线之间的距离。
[0051]
θ
i
：关节转角θ
i
定义为从x
i
‑1旋转到x
i
的角度，绕z
i
轴正向旋转为正。
[0052]
按照d
‑
h模型，可以得到机器人相邻d
‑
h连杆坐标系的齐次变换矩阵：
[0053][0054]
其中为绕轴z
i
旋转θ
i
的旋转变换，为沿着z
i
轴方向移动d
i
的平移变换，为绕轴x
i
旋转α
i
的旋转变换，为沿着x
i
轴方向移动a
i
的平移变换，为第i个d
‑
h连杆坐标系相对于第i
‑
1个d
‑
h连杆坐标系的旋转变换矩阵，
i
‑1p
i
为第i个d
‑
h连杆坐标系的原点在第i
‑
1个d
‑
h连杆坐标系中的坐标。
[0055]
(b)各个连杆质心坐标到各个d
‑
h连杆坐标系下的变换。
[0056]
在某一个给定位姿q＝[θ1，θ2，θ3，θ4，θ5，θ6]
t
下(其中θ1‑
θ6为第一至第六连杆的d
‑
h连杆坐标系z
i
轴的关节转角)，各个连杆在基坐标系下的质心坐标定义为0p
i
，各个连杆在对应d
‑
h连杆坐标系下的质心坐标定义为
i
p
i
。则可以通过旋转变换矩阵和齐次变换矩阵，求出质心在对应d
‑
h连杆坐标系下的坐标：
[0057][0058]
其中，
[0059]
(c)计算在当前位姿下各个关节的电机轴上的等效转动惯量。
[0060]
对于每个关节的电机轴，其绕该连杆坐标系的z
i
轴旋转。在第i个d
‑
h连杆坐标系下，第j个连杆的质心坐标为：
[0061]
[0062]
其中
[0063]
所以第i个关节的电机轴上的等效转动惯量j
i
为：
[0064][0065]
其中
i
x
j
，
i
y
j
为第j个连杆的质心在第i个d
‑
h连杆坐标系下的横纵坐标，为第j个连杆绕过其质心且与z
i
轴平行的轴旋转的转动惯量，m
j
为第j个连杆的质量。
[0066]
以第六连杆为例，根据平行轴定理，由第六关节的电机轴驱动的连杆相对于第六关节的电机轴的转动惯量为：
[0067][0068]
其中m6为第六连杆的质量，6x6、6y6为第六连杆的质心在其d
‑
h连杆坐标系下的横坐标和纵坐标，为第六连杆绕过其质心与z6轴平行的轴旋转的转动惯量。
[0069]
对第五关节的电机轴，其驱动了第六连杆和第五连杆。首先计算第六连杆在第五连杆坐标系下的质心坐标：
[0070][0071]
则可以计算出连杆第五关节的电机轴z5的等效转动惯量：
[0072][0073]
(d)考虑末端负载。
[0074]
在实际的任务中，末端一般会加上负载。当末端加上负载时，为了简化，可以将负载的重量m折算到第六连杆上。
[0075]
在第六个d
‑
h连杆坐标系下考虑：
[0076]
m
′6＝m6 m
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(8)
[0077]6x6′
＝(6x6·
m6 6x
·
m)/(m6 m)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(9)
[0078]6y6′
＝(6y6·
m6 6y
·
m)/(m6 m)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(10)
[0079]6z6′
(6z6·
m
66
z
·
m)/(m6 m)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(11)
[0080]
在将负载的重量折算到第六连杆上之后，将得到的新质心的质量和坐标(m6′
，6x6′
，6y6′
和6z6′
)代替原来第六连杆质心的质量和坐标(m6，6x6，6y6和6z6)。然后代入公式(4)中，则可以得到考虑末端负载时各个关节的电机轴上等效转动惯量。
[0081]
(e)根据电机的输出转矩来计算加速度幅值约束。
[0082]
对于每一个关节，其除了驱动自身的连杆外，还驱动其后一直到末端的所有连杆。第i个关节电机的输出转矩m
i
可以由下式计算：
[0083][0084]
其中j
i
为驱动的连杆i对关节的电机轴线的等效转动惯量，为该关节的电机轴
的角加速度矢量，其方向沿着z
i
轴的轴向，m
f，i
为该关节的摩擦转矩，为关节的电机轴驱动的所有连杆的重力矩之和。
[0085]
设z
i
轴的单位方向矢量为z
i
，则其可以根据对应的旋转变换矩阵的第三列求得。为了将矢量方程(12)标量化，将对应的转矩矢量向z
i
轴上投影，在方程(12)的两侧乘上z
i
：
[0086][0087]
设连杆的角速度方矢量为ω
i
，其方向也沿着z
i
轴的轴向。m
i
＝m
i
·
z
i
为关节电机的驱动力矩，其正负根据连杆的运动方向决定，当ω
i
和z
i
同向时取正，反之取负。m
f，i
＝m
f，i
·
z
i
为摩擦力矩，其正负同样和关节的运动方向相关，当ω
i
和z
i
同向时取负，反之取正。
[0088]
在方程(13)两边取绝对值可得：
[0089][0090]
从上式可得加速度的值是该位姿下关节电机的输出转矩m
i
的一次函数。当ω
i
和z
i
同向时，有0≤m
i
≤m
max
。反之
‑
m
max
≤m
i
≤0。m
max
为关节电机输出转矩的最大值。可以根据当前关节的运动情况，将m
i
的最值代入方程(14)得到对应的姿态下加速度值的最值，从而规划机器人运动时的各个关节的电机轴的加速度值的最大值约束来充分发挥其在不同姿态下的加减速能力。
[0091]
本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。