关节位置描述

乐白机器人LM3为六轴（六个关节）机器人，每个关节的旋转量组成关节位置描述（简称关节位置），可以用六元组表示：

    \[(j_1, j_2, j_3, j_4, j_5, j_6)\]

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

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

    \[(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, \alpha, \beta, \gamma)\]

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

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

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

对于描述坐标系（位姿） {B} ，我们用到如下描述：

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

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

    \[{EulerZYX}(\alpha, \beta, \gamma) = {XYZ}(\gamma, \beta, \alpha) = {RPY}(\gamma, \beta, \alpha)\]

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

    \[{RPY}(\gamma, \beta, \alpha)=\left(\begin{array}{ccc} \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{array}\right)\]

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

    \[\left(\begin{array}{ccc} r_0 & r_1 & r_2 \\\ r_3 & r_4 & r_5 \\ r_6 & r_7 & r_8 \\ \end{array}\right)=RPY(\gamma, \beta, \alpha), \left{\begin{array}{lll} \beta&=\arctan2\left(-r_6, \sqrt{r_0^2+r_3^2}\right), &\beta\in[-\frac{\pi}2, \frac{\pi}2]\ \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{array}\right.\]

特殊地，当 |\beta|=\frac{\pi}{2} 时，\cos\beta=0 。此时取 \gamma=0，有 \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参数对机器人进行运动学描述。

关节 θ d(i) / m a(i-1) / m α(i-1) / rad
1 0 0.21583 0 0
2 0 0 0 1.5708
3 0 0 -0.28 0
4 0 0.12063 -0.26 0
5 0 0.09833 0 1.5708
6 0 0.08343 0 -1.5708

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

奇异位置

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

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

四轴平行

零位