CN108656108A - 四自由度关节型机器人逆运动学的求解方法 - Google Patents

Abstract

本发明公开了一种四自由度关节型机器人逆运动学的求解方法，包括：步骤1：建立动坐标系O‑XZ，建立铰接点A、铰接点C、铰接点B、铰接点E和铰接点F的坐标方程；步骤2：根据机器人的运动学解耦特性，获得铰接点F与铰接点A和铰接点C之间的位置关系坐标方程；步骤3：建立基座坐标系O₀‑X₀Y₀Z₀，通过运动学正求解获得中心点O_N位置正解(X_ON，Y_ON，Z_ON)和速度正解步骤4：求解回转角度θ₁、θ₂，利用位置正解(X_ON，Y_ON，Z_ON)和速度正解获得水平滑块和竖直滑块的位置逆解X_C、Z_A以及速度逆解可简化运动学求解降低了运动学分析的运算复杂度，使整个机器人的反解过程简单直观，提高机械手臂的关节角度精确的控制，提高机器人的安全性。

Description

四自由度关节型机器人逆运动学的求解方法

技术领域

本发明属于工业机器人制造技术领域，更具体地，涉及一种四自由度关节型机器人逆运动学的求解方法。

背景技术

当今，为满足工业生产日益发展的需要，机器人正朝高速、重载及高精度的方向发展，码垛机器人以其在机械结构、适用范围、设备占地空间、灵活性、成本以及维护等方面的优势使其应用渐为广泛，并成为一种发展趋势，解决工业生产中存在的劳动强度大、效率低、安全性差等诸多弊端。

码垛机器人属于圆柱坐标机器人，包括由四个伺服电机分别驱动腰关节、垂直关节、水平关节和腕关节。逆运动学是解决药达成机器人所需要的姿势去设置的各个可活动关节的参数过程，例如，给定一个人体的三维模型，如何设置手腕和手肘的角度以便把手从放松位置变成挥手的姿势，这个问题在机器人学中是很关键的，因为操纵机械手臂通过关节角度来控制。逆运动学在游戏编程和三维建模中也很重要。

因此，有必要提出一种四自由度关节型机器人逆运动学的求解方法。

发明内容

本发明的目的是通过对四自由度码垛机器人各关节处位置和姿态进行求解，得到四自由度关节型机器人逆运动学的求解。

根据本发明提出一种四自由度关节型机器人逆运动学的求解方法，所述四自由度关节型机器人包括底座、腰关节、水平滑块、水平导轨、竖直滑块、竖直导轨、腕关节、末端执行器、前大臂、后大臂、连杆、小臂；

所述腰关节可水平回转的设置于所述底座上，所述水平导轨和所述竖直导轨均连接于所述腰关节，从而在所述腰关节的带动下回转，所述水平滑块能够沿所述导轨移动，所述竖直滑块能够沿所述竖直导轨移动；

所述前大臂的一端通过铰接点C铰接于所述水平滑块，另一端通过铰接点E铰接于所述小臂的中部；

所述后大臂的一端通过铰接点A铰接于所述竖直滑块，另一端通过铰接点D铰接于所述小臂的一端；

所述连杆的一端通过所述铰接点C铰接于所述水平滑块，所述连杆的另一端通过铰接点B铰接于与所述后大臂的中部；

其中，通过所述铰接点C、铰接点B、铰接点D和铰接点E，使得所述前大臂和后大臂之间形成闭合的平行四连杆机构；

所述小臂的另一端通过铰接点F与所述腕关节连接，所述末端执行器可水平回转的设置于所述腕关节上；

其中，所述前大臂与X轴正方向的夹角为α，所述小臂与X轴负方向的夹角为β；BD＝CE＝b，AB＝a，AD＝a+b，DE＝c，EF＝d；腰关节回转角度为θ₁，末端执行器的回转角度为θ₂；

所述求解方法包括如下步骤：

步骤1：建立动坐标系O-XZ，以所述竖直滑块的起始位置为坐标原点O，所述铰接点A、铰接点C、铰接点B、铰接点E和铰接点F的坐标依次为A(0，Z_A)，C(X_C，0)，B(X_B，Z_B)，E(X_E，Z_E)，F(X_F，Z_F)，建立铰接点A、铰接点C、铰接点B、铰接点E和铰接点F的坐标方程；

步骤2：根据所述机器人的运动学解耦特性，获得铰接点F(X_F，Z_F)与铰接点A(0，Z_A)和铰接点C(X_C，0)之间的位置关系坐标方程，使其满足在动坐标系O-XZ所在的平面内实现水平方向和竖直方向上的运动解耦；

步骤3：建立基座坐标系O₀-X₀Y₀Z₀，以腰关节位置为基座坐标系的原点O₀；所述原点O₀为所述坐标原点O沿X轴方向偏移位移d₁和沿Z轴方向偏移位移d₂，所述末端执行器的底面中心点O_N为坐标F(X_F，Z_F)沿X轴方向偏移位移d₃和沿Z轴方向偏移位移d₄；通过运动学正求解获得所述末端执行器的底面中心点O_N在基座坐标系O₀-X₀Y₀Z₀中的位置正解(X_ON，Y_ON，Z_ON)和速度正解

步骤4：求解回转角度θ₁和θ₂，利用位置正解(X_ON，Y_ON，Z_ON)和速度正解获得水平滑块和竖直滑块的位置逆解X_C、Z_A、速度逆解以及所述腰关节的速度逆解

优选地，所述步骤2中的铰接点A、铰接点C、铰接点B、铰接点E和铰接点F的坐标方程如下：

优选地，所述满足在动坐标系O-XZ所在的平面内实现水平和竖直方向上的运动解耦的条件为臂长a·d＝b·c。

优选地，所述步骤2中的铰接点F(X_F，Z_F)与铰接点A(0，Z_A)和铰接点C(X_C，0)之间的位置关系坐标方程为：

优选地，所述末端执行器的底面中心点O_N在动坐标系O-XZ中的坐标方程为：

优选地，所述末端执行器的底面中心点O_N在基座坐标系O₀-X₀Y₀Z₀中的位置正解(X_ON，Y_ON，Z_ON)为：

所述末端执行器的底面中心点O_N在基座坐标系O₀-X₀Y₀Z₀中的速度正解为：

其中：—水平滑块的水平速度；

—竖直滑块的垂直速度；

—腰关节回转的角速度；

优选地，所述步骤4中回转角度θ₁和θ₂为：

其中，θ₁+θ₂＝0。

优选地，所述步骤4中竖直滑块和水平滑块的位置逆解X_C、Z_A为：

所述步骤4中竖直滑块和水平滑块速度逆解以及所述腰关节的速度逆解为：

本发明的有益效果在于：码垛机器人能够通过多个关节形成的平行四连杆机构来实现腕关节沿水平方向伸展和竖直方向俯仰运动，保持末端执行器的水平度，通过对码垛机器人各手臂的关节处的位置和姿态的计算，得到四自由度关节型机器人逆运动学的求解，建立腕关节的铰接点F的位置只与水平滑块和竖直滑块的位置以及腰关节的回转角度有关，即X_F、Z_F的值只取决于X_C、Z_A和θ₁的值，通过这样的关系的建立可大大简化主传动系统机构的运动学求解，且是运动学解耦的，降低了运动学分析的运算复杂度，便于对该机器人进行运动控制，利用末端执行器的底面中心点O_N的位置坐标方程，使整个机器人的水平滑块关节和竖直滑块关节的反解过程简单直观，使得机械手臂的关节角度得到精确的控制，并应用于码垛机器人的码垛和避障等，提高码垛机器人的安全性。

本发明的其它特征和优点将在随后具体实施方式部分予以详细说明。

附图说明

通过结合附图对本发明示例性实施方式进行更详细的描述，本发明的上述以及其它目的、特征和优势将变得更加明显，其中，在本发明示例性实施方式中，相同的参考标号通常代表相同部件。

图1示出了根据本发明的一个实施例的四自由度关节型机器人的结构简图。

具体实施方式

下面将更详细地描述本发明的优选实施方式。虽然以下描述了本发明的优选实施方式，然而应该理解，可以以各种形式实现本发明而不应被这里阐述的实施方式所限制。相反，提供这些实施方式是为了使本发明更加透彻和完整，并且能够将本发明的范围完整地传达给本领域的技术人员。

图1示出了根据本发明的一个实施例的四自由度关节型机器人的结构简图。

如图1所示，本发明的实施例提供一种四自由度关节型机器人逆运动学的求解方法，四自由度关节型机器人包括底座、腰关节、水平滑块、水平导轨、竖直滑块、竖直导轨、腕关节、末端执行器、前大臂、后大臂、连杆、小臂；

腰关节可水平回转的设置于底座上，水平导轨和竖直导轨均连接于腰关节，从而在腰关节的带动下回转，水平滑块能够沿导轨移动，竖直滑块能够沿竖直导轨移动；前大臂的一端通过铰接点C铰接于水平滑块，另一端通过铰接点E铰接于小臂的中部；后大臂的一端通过铰接点A铰接于竖直滑块，另一端通过铰接点D铰接于小臂的一端；连杆的一端通过铰接点C铰接于水平滑块，连杆的另一端通过铰接点B铰接于与后大臂的中部；

其中，通过铰接点C、铰接点B、铰接点D和铰接点E，使得前大臂和后大臂之间形成闭合的平行四连杆机构；小臂的另一端通过铰接点F与腕关节连接，末端执行器可水平回转的设置于腕关节上；

其中，前大臂与X轴正方向的夹角为α，小臂与X轴负方向的夹角为β；BD＝CE＝b，AB＝a，AD＝a+b，DE＝c，EF＝d；腰关节回转角度为θ₁，末端执行器的回转角度为θ₂。

具体地，前大臂的长度为CE＝b，后大臂的中部与连杆的另一端的铰接于点B的距离AB＝a，后大臂的长度为AD＝a+b，小臂的长度为DF＝c+d，连杆的长度为BC＝c，前大臂的另一端与小臂的中部铰接于点E的距离为DC＝c，小臂的中部铰接点E与腕关节的铰接点F的距离为EF＝d，腰关节绕Z轴回转角度为θ₁，末端执行器绕Z轴的回转角度为θ₂。

码垛机器人能够通过各关节形成的平行四连杆机构来实现末端执行器沿水平方向伸展和竖直方向俯仰运动，保持末端执行器的水平度，通过对码垛机器人各手臂的关节处的位置和姿态的计算，实现四自由度关节型机器人的运动学逆求解。

求解方法包括如下步骤：

步骤1：建立动坐标系O-XZ，以竖直滑块的起始位置为坐标原点O，铰接点A、铰接点C、铰接点B、铰接点E和铰接点F的坐标依次为A(0，Z_A)，C(X_C，0)，B(X_B，Z_B)，E(X_E，Z_E)，F(X_F，Z_F)，建立铰接点A、铰接点C、铰接点B、铰接点E和铰接点F的坐标方程；

设定一个固连于机器人支架上的动坐标系O-XZ，该动坐标系中码垛机器人的自由度为2，包括水平滑块的水平移动副和竖直滑块的垂直移动副，当各手臂的杆长度一定时，腕关节的铰接点F的运动规律取决于移动副竖直滑块和水平滑块的运动规律。当码垛机器人的腰关节水平回转时，手臂的各关节绕Z轴转动，调整货物的摆放位置；当末端执行器水平回转时，能够调整货物在水平面的摆放角度。

作为优选方案，步骤2中的铰接点A、铰接点C、铰接点B、铰接点E和铰接点F的坐标方程如下：

步骤2：根据机器人的运动学解耦特性，获得铰接点F(X_F，Z_F)与铰接点A(0，Z_A)和铰接点C(X_C，0)之间的位置关系坐标方程，使其满足在动坐标系O-XZ所在的平面内实现水平方向和竖直方向上的运动解耦；

具体地，为实现垂直平面内的水平滑块和竖直滑块的移动分别控制腕关节沿水平和垂直方向的位置改变，即在垂直平面内实现运动解耦，需要满足d＝k·c，b＝k·a，其中k为机构的放大系数比值。

利用上述特定的几何尺寸，可以实现码垛机器人的运动解耦，当码垛机器人竖直滑块的移动时，腕关节的移动距离是竖直滑块的位移的k倍；当水平滑块沿X轴移动时，且腕关节的移动距离时水平滑块的移动距离的k+1倍。

作为优选方案，满足在动坐标系O-XZ坐标平面内实现水平和竖直方向上的运动解耦的条件为臂长a·d＝b·c。

当满足臂长a·d＝b·c，且各臂长的几何尺寸一定时，腕关节的铰接点F的位置只与水平滑块的铰接点C(X_C，0)和竖直滑块的铰接点A(0，Z_A)的位置有关。

作为优选方案，步骤2中的铰接点F(X_F，Z_F)与铰接点A(0，Z_A)和铰接点C(X_C，0)之间的位置关系坐标方程为：联立公式(1)～(5)，并令联立a·d＝b·c，得到公式(6)

通过这样的关系建立方程可简化主传动系统机构的运动学求解，且是满足运动学解耦的。

步骤3：建立基座坐标系O₀-X₀Y₀Z₀，以腰关节位置为基座坐标系的原点O₀；原点O₀为坐标原点O沿X轴方向偏移位移d₁和沿Z轴方向偏移位移d₂，末端执行器的底面中心点O_N为坐标F(X_F，Z_F)沿X轴方向偏移位移d₃和沿Z轴方向偏移位移d₄；通过运动学正求解获得末端执行器的底面中心点O_N在基座坐标系O₀-X₀Y₀Z₀中的位置正解(X_ON，Y_ON，Z_ON)和速度正解

根据末端执行器底面中心点O_N的坐标位置，在不考虑各手臂和腰关节绕Z₀转动的情况下，由公式(6)可知末端执行器的底面中心点O_N在动参考系坐标方程。

作为优选方案，末端执行器的底面中心点O_N在动坐标O-XZ中的坐标方程为：

作为优选方案，通过坐标的齐次变换可得末端执行器的底面中心点O_N在基座坐标系O₀-X₀Y₀Z₀中的位置正解(X_ON，Y_ON，Z_ON)为：

由公式(8)得出码垛机器人末端执行器中心坐标在基座坐标系O₀-X₀Y₀Z₀中的坐标值，X_ON和Y_ON的值取决于水平滑块的位置X_C和腰关节回转角度θ₁的值，得出Z_ON由Z_A决定，根据由圆柱坐标机器人的特点，在作轨迹规划时水平滑块、竖直滑块和腰关节不易联动，末端执行器的坐标值随着水平滑块、竖直滑块和腰关节的变化而变化。

末端执行器的底面中心点O_N在基座坐标系O₀-X₀Y₀Z₀中的速度正解为：

其中：—水平滑块的水平速度；

—竖直滑块的垂直速度；

—腰关节回转的角速度；

将公式(9)分解为码垛机器人的速度正解的矩阵相乘表达式：

具体地，通过对公式(9)左右两边求导，其中X_C、Z_A、θ₁、 均为变量，得出末端执行器的底面中心点O_N在基座坐标系O₀-X₀Y₀Z₀中的的码垛机器人的加速度正解:

其中：—水平滑块的水平加速度

—竖直滑块的垂直加速度

—腰关节回动的角加速度

步骤4：求解回转角度θ₁和θ₂，利用位置正解(X_ON，Y_ON，Z_ON)和速度正解获得水平滑块和竖直滑块的位置逆解X_C、Z_A、速度逆解以及腰关节的速度逆解

已知码垛机器人的末端执行器的位置，即X_ON、Y_ON、Z_ON已知，求坐标θ₁、θ₂、X_C、Z_A的运动学逆解。假定码垛机器人在搬运过程中始终能够保持包装箱方位不变，则：θ₁+θ₂＝0，即箱式货物从流水线下来，码垛机器人将其抓起，搬运到堆垛上的过程中，货物姿态不变。

作为优选方案，步骤4中回转角度θ₁和θ₂为：

其中，θ₁+θ₂＝0。

作为优选方案，令θ₁+θ₂＝0，由公式(8)得到，步骤4中竖直滑块和水平滑块的位置逆解X_C、Z_A为：

公式(11)即为码垛机器人机械手臂的位置逆解。从公式(11)看出，θ₁只与X_ON和Y_ON有关，而θ₂正好与θ₁相反，反映了箱式货物姿态相对不变，X_C与X_ON、Y_ON都有关系，且不同的X_ON、Y_ON的值可以对应相同的X_C，Z_A的值只与Z_ON有关，实现运动学解耦，方便伺服电机的控制。

步骤4中竖直滑块和水平滑块速度逆解以及腰关节的速度逆解为：

建立腕关节的位置只与水平滑块和竖直滑块的位置以及腰关节的回转角度有关，即X_F、Z_F的值只取决于X_C、Z_A和θ₁的值，通过这样的关系的建立可大大简化主传动系统机构的运动学求解，且是运动学解耦的，降低了运动学分析的运算复杂度，便于对该机器人进行运动控制，通过对四自由度码垛机器人各关节处位置和姿态进行求解，实现对四自由度码垛机器人运动学反求解，通过对码垛机器手臂的各个关节求解正解坐标，得到末端执行器所在位置，利用末端执行器的底面中心点O_N的位置坐标方程，得出码垛机器人各个关节在码垛过程中的运动学反解，使整个机器人的反解过程简单直观，提高机械手臂的关节角度精确的控制，可用于码垛机器人的避障，提高机器人的安全性。

本领域技术人员应理解，上面对本发明的实施例的描述的目的仅为了示例性地说明本发明的实施例的有益效果，并不意在将本发明的实施例限制于所给出的任何示例。

以上已经描述了本发明的各实施例，上述说明是示例性的，并非穷尽性的，并且也不限于所披露的各实施例。在不偏离所说明的各实施例的范围和精神的情况下，对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。

Claims (8)

1.一种四自由度关节型机器人逆运动学的求解方法，所述四自由度关节型机器人包括底座、腰关节、水平滑块、水平导轨、竖直滑块、竖直导轨、腕关节、末端执行器、前大臂、后大臂、连杆、小臂；

所述腰关节可水平回转的设置于所述底座上，所述水平导轨和所述竖直导轨均连接于所述腰关节，从而在所述腰关节的带动下回转，所述水平滑块能够沿所述导轨移动，所述竖直滑块能够沿所述竖直导轨移动；

所述前大臂的一端通过铰接点C铰接于所述水平滑块，另一端通过铰接点E铰接于所述小臂的中部；

所述后大臂的一端通过铰接点A铰接于所述竖直滑块，另一端通过铰接点D铰接于所述小臂的一端；

所述连杆的一端通过所述铰接点C铰接于所述水平滑块，所述连杆的另一端通过铰接点B铰接于与所述后大臂的中部；

其中，通过所述铰接点C、铰接点B、铰接点D和铰接点E，使得所述前大臂和后大臂之间形成闭合的平行四连杆机构；

所述小臂的另一端通过铰接点F与所述腕关节连接，所述末端执行器可水平回转的设置于所述腕关节上；

其中，所述前大臂与X轴正方向的夹角为α，所述小臂与X轴负方向的夹角为β；BD＝CE＝b，AB＝a，AD＝a+b，DE＝c，EF＝d；腰关节回转角度为θ₁，末端执行器的回转角度为θ₂；

所述求解方法包括如下步骤：

步骤1：建立动坐标系O-XZ，以所述竖直滑块的起始位置为坐标原点O，所述铰接点A、铰接点C、铰接点B、铰接点E和铰接点F的坐标依次为A(0，Z_A)，C(X_C，0)，B(X_B，Z_B)，E(X_E，Z_E)，F(X_F，Z_F)，建立铰接点A、铰接点C、铰接点B、铰接点E和铰接点F的坐标方程；

步骤2：根据所述机器人的运动学解耦特性，获得铰接点F(X_F，Z_F)与铰接点A(0，Z_A)和铰接点C(X_C，0)之间的位置关系坐标方程，使其满足在动坐标系O-XZ所在的平面内实现水平方向和竖直方向上的运动解耦；

步骤3：建立基座坐标系O₀-X₀Y₀Z₀，以腰关节位置为基座坐标系的原点O₀；所述原点O₀为所述坐标原点O沿X轴方向偏移位移d₁和沿Z轴方向偏移位移d₂，所述末端执行器的底面中心点O_N为坐标F(X_F，Z_F)沿X轴方向偏移位移d₃和沿Z轴方向偏移位移d₄；通过运动学正求解获得所述末端执行器的底面中心点O_N在基座坐标系O₀-X₀Y₀Z₀中的位置正解(X_ON，Y_ON，Z_ON)和速度正解

步骤4：求解回转角度θ₁和θ₂，利用位置正解(X_ON，Y_ON，Z_ON)和速度正解获得水平滑块和竖直滑块的位置逆解X_C、Z_A、速度逆解以及所述腰关节的速度逆解

2.根据权利要求1所述的四自由度关节型机器人逆运动学的求解方法，其特征在于，所述步骤2中的铰接点A、铰接点C、铰接点B、铰接点E和铰接点F的坐标方程如下：

3.根据权利要求1所述的四自由度关节型机器人逆运动学的求解方法，其特征在于，所述满足在动坐标系O-XZ所在的平面内实现水平和竖直方向上的运动解耦的条件为臂长a·d＝b·c。

4.根据权利要求3所述的四自由度关节型机器人逆运动学的求解方法，其特征在于，所述步骤2中的铰接点F(X_F，Z_F)与铰接点A(0，Z_A)和铰接点C(X_C，0)之间的位置关系坐标方程为：

5.根据权利要求4所述的四自由度关节型机器人逆运动学的求解方法，其特征在于，所述末端执行器的底面中心点O_N在动坐标系O-XZ中的坐标方程为：

6.根据权利要求5所述的四自由度关节型机器人逆运动学的求解方法，其特征在于，所述末端执行器的底面中心点O_N在基座坐标系O₀-X₀Y₀Z₀中的位置正解(X_ON，Y_ON，Z_ON)为：

所述末端执行器的底面中心点O_N在基座坐标系O₀-X₀Y₀Z₀中的速度正解为：

其中：—水平滑块的水平速度；

—竖直滑块的垂直速度；

—腰关节回转的角速度。

7.根据权利要求6所述的四自由度关节型机器人逆运动学的求解方法，其特征在于，所述步骤4中回转角度θ₁和θ₂为：

其中，θ₁+θ₂＝0。

8.根据权利要求7所述的四自由度关节型机器人逆运动学的求解方法，其特征在于，所述步骤4中竖直滑块和水平滑块的位置逆解X_C、Z_A为：

所述步骤4中竖直滑块和水平滑块速度逆解以及所述腰关节的速度逆解为：