机器人笛卡尔空间坐标系轨迹规划的研究

https://blog.csdn.net/weixin_39127054/article/details/79684540

1 引言
随着6R机械人的使用性越来越广，对机械人的轨迹规划的要求也越来越高，机器人轨迹规划是机器人执行作业任务的基础。使用的轨迹规划算法，直接决定了机器人的运动轨迹、平滑程度，且使点位运动有足够高的精度且满足规划路径的约束条件。

本文将在对机器人完成建模以及运动学解算后的基础上，对机器人进行笛卡尔空间轨迹规划的研究，主要分为直线轨迹规划与圆弧轨迹规划。

 

2 任务位置规划
  2.1直线轨迹规划

插补可以分为定时插补和定距插补，定时插补是指每隔固定时间周期插补一个插补点，简单易用，但是缺点是如果示教点过远，即定时插补出来的两个插补点距离较远，精度降低，但可以通过多取示教点或插入中间点来解决。定距插补是指每隔固定距离就插补一个点，只要插补的距离足够近，插补出来的曲线精度就越高，但是这个插补方法是恒定距离，对于不同速度就有不同的时间，难以实现，对算法要求较高，所以一下的研究均采用定时插补。

      

 图1    笛卡尔坐标系下两点

 

笛卡尔空间直线规划算法是在已知直线的始末两点的位置和姿态，求各轨迹插补点的位姿，大多数情况下机器人沿直线运动时其姿态不变，所以无姿态插补，而对于那些要求姿态改变的情况，就需要进行姿态插补。如图1所示，已知直线始末两点M，N相对于基坐标系的坐标姿态，当然这些已知的位置和姿态通常是可以通过示教方式直接得到的。假设v为要求的沿直线的速度，t为插补的时间间隔，我们可以求出直线长度L，t间隔内的距离d以及插补数N如下：

 

那么相邻插补点各轴增量与各轴插补点坐标值如下：

 

相邻插补点增量：                                                                插补点的坐标： 

                                                                        

其中：i = 0,1,2…N

至此机器人的直线规划已完成。

 

2.1圆弧轨迹规划

三维空间中任意不共线的三点可以确定一个空间圆，空间圆和直线一样，都是机器人空间作业轨迹的重要元素之一，在规划其他较为复杂的轨迹时，都扮演着非常基础却很重要的角色。在笛卡尔空间圆弧的轨迹规划中，为了计算方便，必须运用到坐标系变换，也就是说要先在圆弧所在圆弧所在平面建立一个新的直角坐标系，在这新的直角坐标系中计算出圆弧的各插补点在新坐标中的值，最后将所得的中间插补点再经过变换矩阵来转变至用笛卡尔空间来描述。

三点确定一段圆弧，设机器人末端执行器从起始位置P1(x1,y1,z1)经过中间点P2(x2,y2,z2)最后到达终点P3(x3,y3,z3)，如果这三点不共线，就一定存在从起始点P1经中间点P2到终点P3的圆弧。现介绍圆弧轨迹规划的算法步骤：

（1）先求出圆心P0(X0,Y0,Z0)及其半径

（2）以圆弧所在平面建立O1-UVW坐标系，并求出该坐标与基座标系的映射关系

（3）判断圆弧轨迹，求出圆弧的总角度

（4）利用三角函数的关系，求出每个插补点的坐标，并把它映射到基座标系

    

图2    建立O1-UVW坐标系

 

(1)进行第一个步骤：

已知空间中一平面方程的基本形式为：

K11x + k12y + k13z + k14 = 0

 

由P1 (x1,y1,z1)，P2(x2,y2,z2) 和P3(x3,y3,z3)三点确定一平面M，其平面方程可写列出行列式：

 

 

将其展开：

[(y1 – y3)(z2 – z3) – (y2 – y3) (z1 – z3)] (x – x3) +

[(x2 – x3)(z1 – z3) – (x1 – x3) (z2 – z3)] (y – y3) +

[(x1 – x3)(y2 – y3) – (x2 – x3)(y1 – y3)] (z – z3) = 0

 

将上式化简可得：

K11  =  (y1 – y3)(z2 – z3) – (y2 – y3) (z1 – z3)

K12  =  (x2 – x3)(z1 – z3) – (x1 – x3) (z2 – z3)

K13  =  (x1 – x3)(y2 – y3) – (x2 – x3)(y1 – y3)

K14  =  –(K11x3 + K12y3 + K13z3)

 

过P1，P2的中点且与P1 P2垂直的平面T的方程为：

K21x + k22y + k23z + k24 = 0     

式中：K21 = (x2 – x1)，K22 = (y2 – y1)，K23 = (z2 – z1)，K24 = 

 

过P2，P3的中点且与P2 P3垂直的平面S的方程为：

K31x + k32y + k33z + k34 = 0

式中：K31 = (x3 – x2)，K32 = (y3 – y2)，K33 = (z3 – z2)，K34 =

 

空间圆的圆心为平面M，T，S的交点。联立三个平面方程，则可列出P1，P2，P3的外接圆的圆心坐标P0 = [x0  y0  z0]的方程：

 

由此可求出圆心坐标

 

 

进而可求出外接圆的半径：

至此已完成第一个步骤

 

(2)  进行第二步骤：

建立圆弧所在平面O1—UVW的新坐标系，进行坐标系变换

以O1为原点建立新的坐标系O1—UVW，O1W的方向与平面M的法矢量方向一致。由于平面M的法矢量的方向数分别为k11，k12，k13，则O1W轴在基坐标系中的方向余弦为：

 

 

令O1T轴的方向与O1 P1的矢量方向一致，则O1T轴在基坐标系中的方向余弦为：

按右手法则，O1V轴在基坐标系中的方向余弦为O1W轴和O1U轴单位向量的叉乘方向，即v = w x u

 

利用新坐标系O1—UVW的三个坐标轴在基坐标系中的方向余弦u，v，w，即可建立基坐标系O—XYZ到新坐标系O1—UVW的变换矩阵TR为：

 

其逆矩阵TR-1可由齐次变换矩阵求逆得到，记：

  

可以得到逆矩阵TR-1：

 

 

由此可见，基坐标系下的空间圆，在新坐标系O1—UVW下实质上是平面M上的平面圆，两者关系可通过变换矩阵来相互转换。至此两个坐标系的变换关系已求出。

 

（3）进行第三个步骤：

将P1，P2，P3，P0从基坐标系中的值变换到O1—UVW新坐标系中，设原来坐标系中的值分别为P1 (x1,y1,z1)，P1(x2,y2,z2)，P1(x3,y3,z3)，P0(x0,y0,z0)新坐标系中的值分别为(u1,v1,w1)，(u2,v2,w2)，(u3,v3,w3)，(u0,v0,w0)，则有下列求解式子：

                         

由前面推导可知：u0 = v0 = w0 = w1 = w2 = w3 = 0，u1 = r

在新坐标系上进行圆弧插补

       

 

①在对空间的圆弧轨迹规划之前，现在坐标系按照O1—UVW中作出平面M上的圆弧轨迹规划，得到空间圆弧上各点在新坐标系O1—UVW中的位置，然后利用坐标变换，得到空间圆弧上这些点在基坐标系中的位置，于是就完成了空间圆弧轨迹规划。

在把新坐标系O1—UVW内的圆弧变换为基坐标系的空间圆弧轨迹之前，需要确定圆弧的旋转角度及方向，P1，P2，P3的顺序确定圆弧的旋转方向，在新坐标系O1—UVW内确定O1P2，O1P3与U轴（即O1P1）之间的夹角大小：

 

在C#.NET编程语言中，内部数学函数Math.Atan2(y,x)弧度计算取值在-180~180度之间，所以上式中，当atan2(P2y , P2x) < 0时，λ1为1，否则取λ1为0，当atan2(P3y , P3x) < 0时，λ2为1，否则取λ2为0。比较∠P2O1P1与∠P3O1P1的大小，可以确定从P1依次运动到P2，P3的时候，圆弧运动的方向。当∠P2O1P1  <  ∠P3O1P1时，为逆时针方向运动，当∠P2O1P1  > ∠P3O1P1时，为顺时针方向运动。

图中的空间圆弧在坐标系O1—UVW中的方向即为逆时针方向。逆时针方向时，圆弧对应的角度大小为θ=∠P3O1P1，顺时针方向时，圆弧对应的角度大小为θ=2π-∠P3O1P1。

至此已求出规划圆弧的总角度。

 

（4）进行第四个步骤：

通过以上的公式即可以得到空间圆弧在新坐标系O1—UVW中的圆心位置，半径大小，圆弧所对应的角度，以及运动的方向，接着在新坐标系中将圆弧进行插值，计算各插补点。以P1为起始点，通过P2和P3点

的平面圆弧插值方程如下：（i表示第i个插补点,i = 1,2,3......）

  ，其中ui = r * cos(ωt * dir) , vi = r * sin(ωt * dir) , w = 0;

式中，dir为圆弧运动的方向系数，逆时针方向时dir = 1，顺时针方向时dir = -1，ω为圆弧运动的角速度，t∈(0，tall)为运动时间，tall = θ /ω为总运动时间，总插补数（取整数）为。

插补结果得到坐标映射至基座标系上。设点Pi在基坐标系的坐标值为(xi，yi，zi)，则有：

将新坐标系O1—UVW中的位置插值量P通过上式的坐标转换关系转变至基坐标系中，把各插补点的位置和姿态通过运动学逆解，则可以得到各个插补点对应的关节角。

至此已完成了笛卡尔空间中的圆弧规划。