位置和姿态

关节位置描述

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

(j_1, j_2, j_3, j_4, j_5, j_6)

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

在 Lua API 中，用指定索引键的关联数组表示。

在 Lua SDK 中，用数字顺序索引的数组表示。

例如对于关节位置

(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)

在 Lua API 中，可以表示为：

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

在 Lua SDK 中，可以表示为：

{0.024, math.rad(-15), math.pi, math.pi/2, math.rad(30), 0}

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

笛卡尔空间位姿描述

乐白机器人末端的笛卡尔位置和姿态（简称空间位置或位姿）用六元组表示：

(x, y, z, \alpha, \beta, \gamma)

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

在 Lua API 中，用数字顺序索引的数组表示。

在 Lua SDK 中，用指定索引键的关联数组表示。

例如对于笛卡尔空间位姿

(x=0.24\,\mathrm{m}, y=0, z=0, \alpha=\frac{\pi}2, \beta=30^\circ, \gamma=0.024\,\mathrm{rad})

在 Lua API 中，可以表示为：

{0.2, 0, 0, math.pi/2, math.rad(30), 0.024}

在 Lua SDK 中，可以表示为：

{x=0.2, y=0, z=0, rz=math.pi/2, ry=math.rad(30), rx=0.024}

对于描述坐标系（位姿） $\{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)=\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的公式：

\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}

特殊地，当 $|\beta|=\dfrac{\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）	$\alpha_{i-1}$ （单位：rad）
1	0.21583	0	0
2	0	0	1.5708
3	0	-0.28	0
4	0.12063	-0.26	0
5	0.09833	0	1.5708
6	0.08343	0	-1.5708

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

奇异位置

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

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

# 位置和姿态

# 关节位置描述

# 笛卡尔空间位姿描述

# 机器人运动学