前言

本文将介绍笛卡尔空间规划方法。

说明：失踪人口回归，这段时间一直在帮导师写机器人运动规划方面的东西，之前从来没有接触过这方面的知识，通过不断地查资料看论文对机器人运动规划有了一些浅薄的认知。想通过这篇博客记录下学习到的笛卡尔空间规划的知识。

笛卡尔空间规划介绍

在机械臂的实际应用中，通常要求机械臂末端执行器沿着指定的路径（如直线、平面曲线或空间曲线等）进行运动，或者在多点之间进行移动。笛卡尔空间规划就是就是来规划机械臂末端执行器在工作空间运动的轨迹，这个轨迹不光规划了末端执行器所在空间中的位置信息还规划了末端执行器在空间中的姿态。

笛卡尔空间进行规划主要分为以下几步：

第一步，确定末端执行器需要经过的关键点，关键点的确定最简单的可以通过对机械臂进行示教的方式实现；还可以通过一些避障算法实现，因为选关键点除去起始点和目标点，无非就是为了选取一些关键点让执行器避开障碍物。

第二步，确定末端执行器在指定的若干关键点处的位姿（比如在进行从上方抓取物体并放置的任务时，当关键点只有起始点和目标点时，起始点和目标点机械臂末端执行器的Z轴需要垂直于物体），位姿确定后我们就得到了关键点在空间中的x，y，z轴坐标和末端执行器在此关键点的偏航角，俯仰角和翻滚角。

第三步，当关键点处位姿确定后就需要确定关键点之间是如何连接的，也就是要对这些离散的点进行插值或者平滑处理来得到连续的笛卡尔空间轨迹。最简单也是最常用的是通过直线、圆弧或者直线加圆弧混合的方式依次对关键点进行连接，更高级的可以使用高阶多项式、样条曲线和参数化曲线（贝塞尔曲线和B样条曲线）等（这些曲线多用于关节空间规划中）。

第四步，由于机械臂由多个关节组成即使得到了机械臂末端执行器的路径并不能直接进行控制，还需要把路径分成多个点，并把每个点进行运动学逆解得到对应的关节角度，这样就可以通过控制关节来实现对末端执行器的控制。所以到底要取多少个点需要自己定，点越多肯定和曲线越接近，但进行运动学逆解的运算量就越大（不是一般的大O.O）。

下面将介绍直线插值和圆弧插值，更高级的方法在笛卡尔空间的应用在这先挖个坑后面会补上哦。

直线插值

在机械臂笛卡尔空间规划中，直线插值是较为重要的插值方法。如图1所示，末端执行器将从点$P$直线运动到点$Q$，假设移动速度为$v$，位姿插补的时间间隔为$T$，点$P$的坐标为$P\left[ { {x_0},{y_0},{z_0} } \right]$，点$Q$的坐标为$Q\left[ { {x_n},{y_n},{z_n} } \right]$。

空间直线

图1 空间直线

点$P$到点$Q$的距离$L$如式（1）所示。

距离\$L\$公式

$$L = \sqrt { { {({x_n} - {x_0})}^2} + { {({y_n} - {y_0})}^2} + { {({z_n} - {z_0})}^2} } $$

式(1)

末端执行器单次时间间隔$T$内的行程$d$如式（2）所示。

行程\$d\$公式

$$d = vT$$

式(2)

插值总次数$N$如式（3）所示。

插值总次数\$N\$公式

$$N = ceil\left( {\frac{L}{d} } \right)$$

式(3)

其中，$ceil()$为向上取整函数。

注：这里我认为插值次数也可以自己定不一定要按公式来。

各坐标轴增量如式（4）所示。

各坐标轴增量公式

$$\left\{ \begin{array}{l} \Delta x = \frac{ { {x_n} - {x_0} } }{N}\\ \Delta y = \frac{ { {y_n} - {y_0} } }{N}\\ \Delta z = \frac{ { {z_n} - {z_0} } }{N} \end{array} \right.$$

式(4)

$T_i()$时刻机械臂末端执行器在空间坐标系中的坐标值如式（5）所示。

\$T_i()\$时刻机械臂末端执行器在空间坐标系中的坐标值公式

$$\left\{ \begin{array}{l} x = {x_{i - 1} } + i\Delta x\\ y = {y_{i - 1} } + i\Delta y\\ z = {z_{i - 1} } + i\Delta z \end{array} \right.$$

式(5)

圆弧插值

在空间中，通过不在同一直线上的三个点可确定一个圆及三点间的圆弧。假设机械臂笛卡尔空间中的三个节点分别是圆弧的起点$P\left[ { {x_p},{y_p},{z_p} } \right]$，圆弧的中间点$Q\left[ { {x_q},{y_q},{z_q} } \right]$，圆弧的终点$R\left[ { {x_r},{y_r},{z_r} } \right]$，圆弧的圆心$C\left[ { {x_c},{y_c},{z_c} } \right]$，圆弧的半径为$r$，如图2所示。

空间三点及相关参数

图2 空间三点及相关参数

向量$PQ$和向量$QR$分别如式（6）和式（7）所示。

向量\$PQ\$公式

$$PQ = {[\begin{array}{*{20}{c} } { {x_q} - {x_p} }&{ {y_q} - {y_p} }&{ {z_q} - {z_p} } \end{array}]^{\rm{T} } }$$

式(6)

向量\$QR\$公式

$$QR = {[\begin{array}{*{20}{c} } { {x_r} - {x_q} }&{ {y_r} - {y_q} }&{ {z_r} - {z_q} } \end{array}]^{\rm{T} } }$$

式(7)

则$P$、$Q$、$R$三点所在平面$I$的法向量$n$如式（8）所示。

法向量\$n\$公式

\[n = {\left[ {\begin{array}{*{20}{c} } { {n_x} }&{ {n_y} }&{ {n_z} } \end{array} } \right]^{\rm{T} } } = PQ \times QR = \left| {\begin{array}{*{20}{c} } i&j&k\\ { {x_q} - {x_p} }&{ {y_q} - {y_p} }&{ {z_q} - {z_p} }\\ { {x_r} - {x_q} }&{ {y_r} - {y_q} }&{ {z_r} - {z_q} } \end{array} } \right|\]

式(8)

由圆的性质可知，线段$PQ$的中垂线和线段$QR$的中垂线的交点即为圆弧的圆心。

同时垂直于向量$n$和向量$PQ$的向量即为线段$PQ$的中垂线的方向向量$n_1$，$n_1$如式（9）所示。

方向向量\$n_1\$公式

\[{n_1} = {\left[ {\begin{array}{*{20}{c} } { {n_{1x} } }&{ {n_{1y} } }&{ {n_{1z} } } \end{array} } \right]^{\rm{T} } } = n \times PQ = \left| {\begin{array}{*{20}{c} } i&j&k\\ { {n_x} }&{ {n_y} }&{ {n_z} }\\ { {x_q} - {x_p} }&{ {y_q} - {y_p} }&{ {z_q} - {z_p} } \end{array} } \right|\]

式(9)

假设线段$PQ$的中点为$A\left[ { {a_x},{a_y},{a_z} } \right]$，由$P$点和$Q$点的坐标可得$a_x$、$a_y$、$a_z$的值如式（10）所示。

点\$A\$坐标公式

\[{a_x} = \frac{ { {x_q} + {x_p} } }{2},{a_y} = \frac{ { {y_q} + {y_p} } }{2},{a_y} = \frac{ { {z_q} + {z_p} } }{2}\]

式(10)

线段$PQ$的中垂线所在直线的方程$L_1$如式（11）所示。

直线\$L_1\$方程

\[\frac{ {x - {a_x} } }{ { {n_{1x} } } } = \frac{ {y - {a_y} } }{ { {n_{1y} } } } = \frac{ {z - {a_z} } }{ { {n_{1z} } } }\]

式(11)

同时垂直于向量$n$和向量$QR$的向量即为线段$QR$的中垂线的方向向量$n_2$，$n_2$如式（12）所示。

方向向量\$n_2\$公式

\[{n_2} = {\left[ {\begin{array}{*{20}{c} } { {n_{2x} } }&{ {n_{2y} } }&{ {n_{2z} } } \end{array} } \right]^{\rm{T} } } = n \times PQ = \left| {\begin{array}{*{20}{c} } i&j&k\\ { {n_x} }&{ {n_y} }&{ {n_z} }\\ { {x_r} - {x_q} }&{ {y_r} - {y_q} }&{ {z_r} - {z_q} } \end{array} } \right|\]

式(12)

假设线段$QR$的中点为$B\left[ { {b_x},{b_y},{b_z} } \right]$，由$Q$点和$R$点的坐标可得$b_x$、$b_y$、$b_z$的值如式（13）所示。

点\$B\$坐标公式

\[{b_x} = \frac{ { {x_r} + {x_q} } }{2},{b_y} = \frac{ { {y_r} + {y_q} } }{2},{b_y} = \frac{ { {z_r} + {z_q} } }{2}\]

式(13)

线段$QR$的中垂线所在直线的方程$L_2$如式（14）所示。

直线\$L_2\$方程

\[\frac{ {x - {b_x} } }{ { {n_{2x} } } } = \frac{ {y - {b_y} } }{ { {n_{2y} } } } = \frac{ {z - {b_z} } }{ { {n_{2z} } } }\]

式(14)

由圆的性质可知，直线$L_1$和$L_2$的交点即为圆弧轨迹的圆心。可以采用线性方程组解法，将直线方程$L_1$和$L_2$转化成空间直线的一般式，由于方程组个数大于未知量个数，故利用广义逆来求$L_1$和$L_2$的交点，即圆弧的圆心坐标如式（15）所示。

圆弧圆心\$C\$坐标

\[\left[ {\begin{array}{*{20}{c} } { {x_c} }\\ { {y_c} }\\ { {z_c} } \end{array} } \right] = {\left[ {\begin{array}{*{20}{c} } { {\rm{ } }{n_{1y} }{\rm{\ \ \ } }{n_{1x} }{\rm{\ \ \ } }0{\rm{\ \ } } }\\ { {\rm{\ } }0{\rm{\ \ \ } }{n_{1z} }{\rm{\ \ \ } }{n_{1y} } }\\ { {n_{2y} }{\rm{\ \ \ } }{n_{2x} }{\rm{\ \ \ } }0{\rm{\ \ } } }\\ { {\rm{\ } }0{\rm{\ \ \ \ } }{n_{2z} }{\rm{\ \ \ } }{n_{2y} } } \end{array} } \right]^ + } \cdot \left[ {\begin{array}{*{20}{c} } { {a_x}{n_{1y} } - {n_{1x} }{a_y} }\\ { {a_y}{n_{1z} } - {a_z}{n_{1y} } }\\ { {b_x}{n_{2y} } - {n_{2x} }{b_y} }\\ { {b_y}{n_{2z} } - {b_z}{n_{2y} } } \end{array} } \right]\]

式(15)

求得了圆弧的圆心坐标$\left[ { {x_c},{y_c},{z_c} } \right]$，则$CP$、$CQ$、$CR$三个向量的模都等于圆弧的半径$r$，如式（16）所示。

圆弧半径\$r\$公式

\[r = \left| {CP} \right| = \left| {CQ} \right| = \left| {CR} \right|\]

式(16)

将空间圆弧转换为平面圆弧。如图3所示，建立坐标系{$O’$$X’$$Y’$$Z’$}，使原点$O’$位于圆心$C$处，以向量$CP$所在直线为$X’$轴，方向与向量$CP$相同，过点$C$且垂直于平面$I$的直线为$Z’$轴，$Y’$轴由右手定则确定，且平面$X’$$O’$$Y’$与平面$I$重合。

平面坐标系定义

图3 平面坐标系定义

假设$X’$轴上的单位向量为$i’$，它在坐标系{$O$$X$$Y$$Z$}中如式（17）所示。

单位向量\$i’\$公式

\[i' = {\left[ {\begin{array}{*{20}{c} } {\frac{ { {x_p} - {x_c} } }{ {\left| {CP} \right|} } }&{\frac{ { {y_p} - {y_c} } }{ {\left| {CP} \right|} } }&{\frac{ { {z_p} - {z_c} } }{ {\left| {CP} \right|} } } \end{array} } \right]^{\rm{T} } }\]

式(17)

假设$Z’$轴上的单位向量为$k’$，它在坐标系{$O$$X$$Y$$Z$}中如式（18）所示。

单位向量\$k’\$公式

\[k' = \frac{n}{ {\left| n \right|} } = {\left[ {\begin{array}{*{20}{c} } {\frac{ { {n_x} } }{ {\left| n \right|} } }&{\frac{ { {n_y} } }{ {\left| n \right|} } }&{\frac{ { {n_z} } }{ {\left| n \right|} } } \end{array} } \right]^{\rm{T} } }\]

式(18)

假设$Y’$轴上的单位向量为$j’$，它在坐标系{$O$$X$$Y$$Z$}中如式（19）所示。

单位向量\$j’\$公式

\[\begin{array}{l} j' = k' \times i'\\ {\rm{\ \ } } = {\left[ {\begin{array}{*{20}{c} } {\frac{ { {y_p} - {y_c} } }{ {\left| {CP} \right|} } \cdot \frac{ { {n_x} } }{ {\left| n \right|} } - \frac{ { {z_p} - {z_c} } }{ {\left| {CP} \right|} } \cdot \frac{ { {n_y} } }{ {\left| n \right|} } }&{\frac{ { {z_p} - {z_c} } }{ {\left| {CP} \right|} } \cdot \frac{ { {n_x} } }{ {\left| n \right|} } - \frac{ { {x_p} - {x_c} } }{ {\left| {CP} \right|} } \cdot \frac{ { {n_z} } }{ {\left| n \right|} } }&{\frac{ { {x_p} - {x_c} } }{ {\left| {CP} \right|} } \cdot \frac{ { {n_y} } }{ {\left| n \right|} } - \frac{ { {y_p} - {y_c} } }{ {\left| {CP} \right|} } \cdot \frac{ { {n_x} } }{ {\left| n \right|} } } \end{array} } \right]^{\rm{T} } } \end{array}\]

式(19)

则坐标系{$O’$$X’$$Y’$$Z’$}到坐标系{$O$$X$$Y$$Z$}之间的转换矩阵${}_o^{o’}T$如式（20）所示。

转换矩阵\${}_o^{o'}T\$公式

\[{}_o^{o'}T = \left[ {\begin{array}{*{20}{c} } {\frac{ { {x_p} - {x_c} } }{ {\left| {CP} \right|} } }&{\frac{ { {y_p} - {y_c} } }{ {\left| {CP} \right|} } \cdot \frac{ { {n_z} } }{ {\left| n \right|} } - \frac{ { {z_p} - {z_c} } }{ {\left| {CP} \right|} } \cdot \frac{ { {n_y} } }{ {\left| n \right|} } }&{\frac{ { {n_x} } }{ {\left| n \right|} } }&{ {x_c} }\\ {\frac{ { {y_p} - {y_c} } }{ {\left| {CP} \right|} } }&{\frac{ { {z_p} - {z_c} } }{ {\left| {CP} \right|} } \cdot \frac{ { {n_x} } }{ {\left| n \right|} } - \frac{ { {x_p} - {x_c} } }{ {\left| {CP} \right|} } \cdot \frac{ { {n_z} } }{ {\left| n \right|} } }&{\frac{ { {n_y} } }{ {\left| n \right|} } }&{ {y_c} }\\ {\frac{ { {z_p} - {z_c} } }{ {\left| {CP} \right|} } }&{\frac{ { {x_p} - {x_c} } }{ {\left| {CP} \right|} } \cdot \frac{ { {n_y} } }{ {\left| n \right|} } - \frac{ { {y_p} - {y_c} } }{ {\left| {CP} \right|} } \cdot \frac{ { {n_x} } }{ {\left| n \right|} } }&{\frac{ { {n_z} } }{ {\left| n \right|} } }&{ {z_c} }\\ 0&0&0&1 \end{array} } \right]\]

式(20)

则坐标系{$O’$$X’$$Y’$$Z’$}中任意点的坐标($x’$,$y’$,$z’$)，则它在坐标系{$O$$X$$Y$$Z$}中对应点($x$,$y$,$z$)如式（21）所示。

两坐标系点的对应公式

\[\left[ {\begin{array}{*{20}{c} } {x'}\\ {y'}\\ {z'}\\ 1 \end{array} } \right] = {}_o^{o'}{T^{ - 1} }\left[ {\begin{array}{*{20}{c} } x\\ y\\ z\\ 1 \end{array} } \right]\]

式(21)

同样，已知坐标系{$O$$X$$Y$$Z$}中任意点的坐标($x$,$y$,$z$)，则该点在坐标系{$O’$$X’$$Y’$$Z’$}中对应点的坐标($x’$,$y’$,$z’$)如式（22）所示。

内积\$H\$公式

\[\left[ {\begin{array}{*{20}{c} } {x'}\\ {y'}\\ {z'}\\ 1 \end{array} } \right] = {}_o^{o'}{T^{ - 1} }\left[ {\begin{array}{*{20}{c} } x\\ y\\ z\\ 1 \end{array} } \right]\]

式(22)

利用上式就能得到$P$、$Q$、$R$、$C$四个点在坐标系{$O’$$X’$$Y’$$Z’$}中对应点的坐标。

得到坐标后就可以进行圆弧插补轨迹的规划，首先在平面坐标系{$O’$$X’$$Y’$$Z’$}中，计算圆弧$PQR$的圆心角$θ$和插值次数$N$。如图3所示，圆心角$θ$可能有两种情况：$θ≤π$和$θ>π$。这里，利用向量$O’P$和$PR$的法向量$n_3$与平面$X’O’Y’$的法向量$n$的内积来判断$θ$的值,如式（23）所示。

内积\$H\$公式

\[H = {n_3} \cdot n = O'P \times PR \cdot n\]

式(23)

若$H≥0$，则$θ≤π$，$\theta = 2\arcsin \left( {\frac{ {\left| {PR} \right|} }{ {2r} } } \right)$;若$H<0\$，则\$θ>π$，$\theta = 2\pi - 2\arcsin \left( {\frac{ {\left| {PR} \right|} }{ {2r} } } \right)$。

假设机械臂的插补周期为$t$，移动速度为$v$，则机械臂末端执行器从当前点切向移动到下一个插补点的距离如式（24）所示。

间距公式

\[\Delta s = vt\]

式(24)

步距角$δ$可如式（25）所示。

步距角\$δ\$公式

\[\delta = \arcsin \left( {\frac{ {vt} }{r} } \right)\]

式(25)

插值总次数$N$如式（26）所示。

插值总次数\$N\$公式

$$N = ceil\left( {\frac{\theta }{\delta } } \right)$$

式(26)

其中，$ceil()$为向上取整函数。

然后，在坐标系{$O’$$X’$$Y’$$Z’$}中，从$P$点开始，依次计算出圆弧轨迹上每个插补点的位置坐标($x’$,$y’$,$z’$)，再利用坐标变换矩阵${}_o^{o’}T$求得该点在机械臂参考坐标系{$O$$X$$Y$$Z$}中的坐标($x$,$y$,$z$)，这样就完成了机械臂圆弧插补轨迹的规划。

既然有了直线和圆弧的插值方法，那将直线插值方法和圆弧插值方式进行组合使用也就很好实现啦。（这里先偷个懒，可能等后面有时间再补上吧）

当然了还有其他的一些高级方法哦。（这里也先偷个懒，主要还是这些方法有些细节我还没弄明白先弄明白再来写）

姿态控制规划

注：简单介绍下，感觉不是很详细，后面补充

对于机械臂的姿态控制规划，特别是在执行中间路径时，有几种方法可以实现。以下是一些常见的方法：

插值方法：这是最简单的方法之一，涉及将中间路径上的姿态点进行插值，以便在这些点之间平滑地进行运动。简单来说和前面一样就是把离散的点连接起来，确保机械臂在路径上均匀地变化姿态，从而实现平滑的运动。

举个例子，当我们有了起始姿态和目标姿态后，我们通过在起始姿态和目标姿态之间选择一些中间点，然后对这些点使用相应的插值方法进行插值。

关节空间插补：机械臂通常由一系列关节连接而成。在关节空间中，可以对各个关节的角度进行插值，从而控制机械臂的姿态。这种方法适用于具有较大自由度的机械臂，可以避免姿态的奇异性问题。

举个例子，当我们有了起始姿态和目标姿态后，使用逆运动学，我们可以计算出每个中间点的关节角度，然后对这些关节角度使用相应的插值方法进行插值。

欧拉角/四元数插值：如果机械臂的姿态由欧拉角（滚转、俯仰、偏航）或者四元数表示，可以对这些表示进行插值。这样可以确保在中间路径上平滑地过渡，同时避免姿态的奇异性问题。

举个例子，假设我们使用欧拉角来表示姿态（欧拉角和四元数可以进行转换）。当我们通过正运动学有了起始欧拉角和目标欧拉角后，可以在起始欧拉角和目标欧拉角之间分别对滚转、俯仰和偏航角使用相应的插值方法进行插值。

算法：可以通过一些启发式或者采样的算法，从起始姿态开始，逐步生成到目标姿态的路径。

小结

到此为止我们通过前面的方法得到了大量离散的路径点，后面还需要根据机械臂逆解得到每个点所对应的机械臂每个关节的关节角度，也就是转化为机械臂的关节角度序列，到这里实际上我们已经完成了笛卡尔空间规划。但是要注意一点哦，即使取的点够密集那也是离散的点（比如相邻两点，第一个关节角度分别为30°和30.001°，即使它们相差很小，但他们的关节角度不是连续的），关节角度不可能直接发生瞬间变化所以那就还需要在关节空间对其关节角度进行规划给它一个变化的过渡过程，这就是关节空间规划的任务啦。

注：
其实一开始我一直以为笛卡尔空间规划就只是规划出一条在空间中的路径，其实并不是这样的，因为在这段路径中其实还包括机械臂姿态的问题，也就是机械臂在笛卡尔空间进行轨迹规划的过程需要对末端执行器位置和姿态都进行规划。