位置和姿态

关节位置描述

乐白机器人LM3为六轴(六个关节)机器人,每个关节的旋转量组成关节位置描述(简称关节位置),可以用六元组表示:

(j1,j2,j3,j4,j5,j6) (j_1, j_2, j_3, j_4, j_5, j_6)

其中 jij_i 表示第 i 个关节的旋转角度,单位为弧度(rad)。

在 Lua 代码中,用指定索引键的关联数组表示。例如对于关节位置

(j1=0.024rad,j2=15,j3=π,j4=π2,j5=30,j6=0) (j_1=0.024\,\mathrm{rad}, j_2=-15^\circ, j_3=\pi, j_4=\frac{\pi}2, j_5=30^\circ, j_6=0)

可以表示为:

{j1=0.024, j2=math.rad(-15), j3=math.pi, j4=math.pi/2, j5=math.rad(30), j6=0}

关节描述的范围理论上是无限制的,具体的限制范围取决于具体的应用场景及可能产生的自干涉。

笛卡尔空间位姿描述

乐白机器人末端的笛卡尔位置和姿态(简称空间位置位姿)用六元组表示:

(x,y,z,α,β,γ) (x, y, z, \alpha, \beta, \gamma)

其中 (x,y,z)(x, y, z) 表示空间笛卡尔坐标位置,单位为米(m)。 (α,β,γ)(\alpha, \beta, \gamma) 表示旋转姿态,单位为弧度(rad)。

在 Lua 代码中,用数字顺序索引的数组表示:

{x, y, z, alpha, beta, gamma}

对于描述坐标系(位姿){B}\{B\},我们用到如下描述:

  • X-Y-Z固定角。首先将坐标系{B}\{B\}和已知参考坐标系{A}\{A\}重合。先将{B}\{B\}X^A\hat{X}_A旋转γ\gamma角,再绕Y^A\hat{Y}_A旋转β\beta角,最后绕Z^A\hat{Z}_A旋转α\alpha角。
  • Z-Y-X欧拉角。首先将坐标系{B}\{B\}和已知参考坐标系{A}\{A\}重合。先将{B}\{B\}Z^B\hat{Z}_B旋转α\alpha角,再绕Y^B\hat{Y}_B旋转β\beta角,最后绕X^B\hat{X}_B旋转γ\gamma角。
  • RPY旋转角。X-Y-Z固定角也可定义为RPY旋转角,即回转角RR(roll)、俯仰角PP(pitch)和偏转角YY(yaw)。首先将坐标系{B}\{B\}和已知参考坐标系{A}\{A\}重合。先将{B}\{B\}X^A\hat{X}_A旋转 roll 角,再绕Y^A\hat{Y}_A旋转 pitch 角,最后绕Z^A\hat{Z}_A旋转 yaw 角。

乐白机器人的旋转姿态是用Z-Y-X欧拉角描述的。它们的关系如下:

EulerZYX(α,β,γ)=XYZ(γ,β,α)=RPY(γ,β,α) {EulerZYX}(\alpha, \beta, \gamma) = {XYZ}(\gamma, \beta, \alpha) = {RPY}(\gamma, \beta, \alpha)

当开发者需要在乐白的位姿和其他位姿表示法之间转换时,需要通过选择矩阵进行变换。一般计算机编程中常用RPY或四元组描述姿态,这里给出将RPY转为矩阵的公式:

RPY(γ,β,α)=(cosαcosβcosαsinβsinγsinαcosγcosαsinβcosγ+sinαsinγsinαcosβsinαsinβsinγ+cosαcosγsinαsinβcosγcosαsinγsinβcosβsinγcosβcosγ) {RPY}(\gamma, \beta, \alpha)=\begin{pmatrix} \cos\alpha \cos\beta & \cos\alpha \sin\beta \sin\gamma - \sin\alpha \cos\gamma & \cos\alpha \sin\beta \cos\gamma + \sin\alpha \sin\gamma \\ \sin\alpha \cos\beta & \sin\alpha \sin\beta \sin\gamma + \cos\alpha \cos\gamma & \sin\alpha \sin\beta \cos\gamma - \cos\alpha \sin\gamma \\ -\sin\beta & \cos\beta \sin\gamma & \cos\beta \cos\gamma \end{pmatrix}

给出将3×3旋转矩阵转为RPY的公式:

(r0r1r2r3r4r5r6r7r8)=RPY(γ,β,α),{β=arctan2(r6,r02+r32),β[π2,π2]α=arctan2(r3cosβ,r0cosβ),α[π,π]γ=arctan2(r7cosβ,r8cosβ),γ[π,π] \begin{pmatrix} r_0 & r_1 & r_2 \\ r_3 & r_4 & r_5 \\ r_6 & r_7 & r_8 \\ \end{pmatrix}= RPY(\gamma, \beta, \alpha), \begin{cases} \beta&=\arctan2\left(-r_6, \sqrt{r_0^2+r_3^2}\right), &\beta\in\left[-\dfrac{\pi}2, \dfrac{\pi}2\right] \\ \alpha&=\arctan2\left(\dfrac{r_3}{\cos\beta}, \dfrac{r_0}{\cos\beta}\right) , & \alpha\in[-\pi, \pi] \\ \gamma&=\arctan2\left(\dfrac{r_7}{\cos\beta}, \dfrac{r_8}{\cos\beta}\right) , & \gamma\in[-\pi, \pi] \end{cases}

特殊地,当 β=π2|\beta|=\dfrac{\pi}{2} 时,cosβ=0\cos\beta=0。此时取 γ=0\gamma=0,有 α=arctan2(r1,r4)\alpha=\arctan2(-r_1, r_4)

给出在KDL中的转换实现(C++):

Rotation Rotation::EulerZYX(double Alfa,double Beta,double Gamma) {
    return RPY(Gamma,Beta,Alfa);
}

void Rotation::GetEulerZYX(double& Alfa,double& Beta,double& Gamma) const {
    GetRPY(Gamma,Beta,Alfa);
}

Rotation Rotation::RPY(double roll,double pitch,double yaw) {
    double ca1,cb1,cc1,sa1,sb1,sc1;
    ca1 = cos(yaw); sa1 = sin(yaw);
    cb1 = cos(pitch);sb1 = sin(pitch);
    cc1 = cos(roll);sc1 = sin(roll);
    return Rotation(ca1*cb1,ca1*sb1*sc1 - sa1*cc1,ca1*sb1*cc1 + sa1*sc1,
                    sa1*cb1,sa1*sb1*sc1 + ca1*cc1,sa1*sb1*cc1 - ca1*sc1,
                    -sb1,cb1*sc1,cb1*cc1);
}

// Gives back a rotation matrix specified with RPY convention
void Rotation::GetRPY(double& roll,double& pitch,double& yaw) const {
    double epsilon=1E-12;
    pitch = atan2(-data[6], sqrt( sqr(data[0]) +sqr(data[3]) )  );
    if ( fabs(pitch) > (PI_2-epsilon) ) {
        yaw = atan2(  -data[1], data[4]);
        roll  = 0.0 ;
    } else {
        roll  = atan2(data[7], data[8]);
        yaw   = atan2(data[3], data[0]);
    }
}

机器人运动学

机器人运动,当指定关节位置时,只要不产生自干涉(即自己打到自己),理论上是可以达到任何位置的。当一组关节位置确定,以及TCP设置确定时,机器人的位姿也是确定且唯一的。这一计算过程即机器人正向运动学。

当指定空间位置时,情况就变得大不一样。由于机器人本身的构型,将空间位置转换为关节位置的机器人逆向运动学是多解或者无解的。机器人无法到达空间上任意一点,这是物理的限制。机器人也无法在空间两点之间任意地执行运动——这一现象在机器人处于「奇异位置」时尤其明显,此时执行空间移动命令很有可能导致算法运行失败,机器人不得不停止运动。

当机器人运动中出现如下错误提示时,请修改场景或程序中的运动命令,并通过示教或者关节移动将机器人恢复到正常位置。

  • 1140 机器人运动规划出错
  • 1010 机器人无法到达该位置

D-H参数

乐白机器人使用改进的D-H参数对机器人进行运动学描述。

关节θ\thetadid_i(单位:m)ai1a_{i-1} (单位:m)αi1\alpha_{i-1}(单位:rad)
100.2158300
20001.5708
300-0.280
400.12063-0.260
500.0983301.5708
600.083430-1.5708

由于制造、装配误差的存在,每台机器人的实际DH参数与理论值之间都会存在一定的偏差。乐白在每台机器人出厂前都会对实际DH参数进行精标,需要进行视觉抓取、离线仿真等应用的客户可以向开发人员咨询详细信息。

奇异位置

典型的奇异位置有如下三种:

  1. 四轴平行,即第2、3、4、6关节处于平行状态。
  2. 零位,即机器人的所有关节角度都为0时。
  3. 机器人手臂在任何位置完全伸直时

四轴平行

零位