CN104108101A - 一种新型的机器人关节空间运动规划方法 - Google Patents

Abstract

本发明设计一种新型的机器人关节空间运动规划方法，属于机器人控制领域。其特征在于：当机器人的末端需要进行轨迹跟踪控制，且机器人的运动关节个数大于机器人末端自由度约束个数时，通过对每个运动关节的运动量添加软约束，然后运用信息融合估计方法，可以规划出机器人关节空间的运动轨迹。软约束是对运动关节期望运动量和相应信息量的描述方程，信息融合估计方法是一种对包含信息量的线性矩阵方程的求解方法。本发明可以运用在机器人关节空间的运动规划过程中，通过调节运动关节的信息量，可以调节不同关节的运动量大小。通过不同关节运动量的优化组合，可以提高机器人的整体运动效率和减少机器人的整体能量消耗。

Description

一种新型的机器人关节空间运动规划方法

技术领域

本发明设计一种新型的机器人关节空间运动规划方法，属于机器人控制领域。

背景技术

工业机器人的运动，根据其运动轨迹可以分为点到点运动和轨迹跟踪运动。点到点运动只关心特定的位置点，而路径跟踪则关心整个运动路径。由于点到点运动只关心起始和目标位置点，对运动路径没有限制，所以在点到点运动中，机器人的末端笛卡尔坐标系中具有多条可能的轨迹。

机器人的轨迹跟踪运动是以点到点运动为基础的。轨迹跟踪运动是希望机器人的末端以特定的姿态沿给定的路径运动。为了保证机器人的术端处在给定的路径上，需要计算出路径上各点的位置，以及在各个位置点机器人所需要达到的姿态。在有些应用场合，如果对机器人末端姿态没有要求，则只需要计算路径上各点的位置。上述计算路径上各点处的机器人位置与姿态的过程，称为机器人笛卡尔空间的路径规划。根据规划出的各个路径点处的机器人位置和姿态，利用逆向运动学求取机器人各个关节的目标位置，通过控制各个关节的运动，使机器人的末端到达各个路径点处的期望位置。

可见，轨迹跟踪运动需要在笛卡尔空间对机器人末端的位姿进行运动规划，同时还需要在机器人的关节空间进行运动规划。本发明的目的就是设计一种新型的机器人关节空间运动规划方法，解决对于给定末端运动轨迹的机器人关节空间的运动规划问题。

发明内容

机械手的笛卡尔空间运动速度与关节空间运动速度之间的变换，称为雅可比矩阵。雅可比矩阵是关节空间速度向笛卡尔空间速度的传动比。设x机械手术端位姿的广义位置矢量，是6维矢量。q为机械手的关节坐标矢量，n个关节则为n维矢量。由广义位置矢量与关节坐标矢量之间的关系，可以导出广义速度矢量与关节速度矢量的关系为：

$x=x\left(q\right)\⇒\stackrel{\·}{x}=\underset{i=1}{\overset{6}{\mathrm{\Σ}}}\underset{j=1}{\overset{n}{\mathrm{\Σ}}}\frac{\∂x_i}{\∂q_j}{\stackrel{\·}{q}}_j=J\left(q\right)\stackrel{\·}{q}---\left(1\right)$

其中J(q)为6×n的矩阵。又可以表示为：

$\mathrm{dx}=\left[\begin{array}{cccc}{J}_{11}& J_{12}& \·\·\·& J_{1n}\\ J_{21}& J_{22}& \·\·\·& J_{2n}\\ J_{31}& J_{32}& \·\·\·& J_{3n}\\ J_{41}& J_{42}& \·\·\·& J_{4n}\\ J_{51}& J_{52}& \·\·\·& J_{5n}\\ J_{61}& J_{62}& \·\·\·& J_{6n}\end{array}\right]\left[\begin{array}{c}{\stackrel{\·}{q}}_1\\ {\stackrel{\·}{q}}_2\\ \·\\ \·\\ \·\\ {\stackrel{\·}{q}}_{n-1}\\ {\stackrel{\·}{q}}_n\end{array}\right]---\left(2\right)$

对于上述矩阵方程两边同时乘以ΔT，可得：

$\mathrm{\Δx}=\left[\begin{array}{cccc}{J}_{11}& J_{12}& \·\·\·& J_{1n}\\ J_{21}& J_{22}& \·\·\·& J_{2n}\\ J_{31}& J_{32}& \·\·\·& J_{3n}\\ J_{41}& J_{42}& \·\·\·& J_{4n}\\ J_{51}& J_{52}& \·\·\·& J_{5n}\\ J_{61}& J_{62}& \·\·\·& J_{6n}\end{array}\right]\left[\begin{array}{c}{\mathrm{\Δq}}_1\\ \mathrm{\Δ}{q}_2\\ \·\\ \·\\ \·\\ {\mathrm{\Δq}}_{n-1}\\ \mathrm{\Δ}{q}_n\end{array}\right]---\left(3\right)$

假设机器人末端的运动路径为从点A到点B的一条曲线，将该曲线分割成m段，取其中一段为oo′，机器人末端由点o到o′的位置矢量增量为Δx，将机器人末端在点o处对应的关节空间坐标矢量q_o代入式(3)可得，机器人末端在o点的位置矢量增量Δx与关节空间坐标矢量增量Δq_i的关系如下所示：

$\mathrm{\Δx}=\left[\begin{array}{cccc}{J}_{11}\left(q_o\right)& J_{12}\left(q_o\right)& \·\·\·& J_{1n}\left(q_o\right)\\ J_{21}\left(q_o\right)& J_{22}\left(q_o\right)& \·\·\·& J_{2n}\left(q_o\right)\\ J_{31}\left(q_o\right)& J_{32}\left(q_o\right)& \·\·\·& J_{3n}\left(q_o\right)\\ J_{41}\left(q_o\right)& J_{42}\left(q_o\right)& \·\·\·& J_{4n}\left(q_o\right)\\ J_{51}\left(q_o\right)& J_{52}\left(q_o\right)& \·\·\·& J_{5n}\left(q_o\right)\\ J_{61}\left(q_o\right)& J_{62}\left(q_o\right)& \·\·\·& J_{6n}\left(q_o\right)\end{array}\right]\left[\begin{array}{c}{\mathrm{\Δq}}_1\\ \mathrm{\Δ}{q}_2\\ \·\\ \·\\ \·\\ {\mathrm{\Δq}}_{n-1}\\ {\mathrm{\Δq}}_n\end{array}\right]---\left(4\right)$

令 $J\left(q_o\right)=\left[\begin{array}{cccc}{J}_{11}\left(q_o\right)& J_{12}\left(q_o\right)& \·\·\·& J_{1n}\left(q_o\right)\\ J_{21}\left(q_o\right)& J_{22}\left(q_o\right)& \·\·\·& J_{2n}\left(q_o\right)\\ J_{31}\left(q_o\right)& J_{32}\left(q_o\right)& \·\·\·& J_{3n}\left(q_o\right)\\ J_{41}\left(q_o\right)& J_{42}\left(q_o\right)& \·\·\·& J_{4n}\left(q_o\right)\\ J_{51}\left(q_o\right)& J_{52}\left(q_o\right)& \·\·\·& J_{5n}\left(q_o\right)\\ J_{61}\left(q_o\right)& J_{62}\left(q_o\right)& \·\·\·& J_{6n}\left(q_o\right)\end{array}\right],$ $\mathrm{\Δq}=\left[\begin{array}{c}\mathrm{\Δ}{q}_1\\ \mathrm{\Δ}{q}_2\\ \·\\ \·\\ \·\\ {\mathrm{\Δq}}_{n-1}\\ \mathrm{\Δ}{q}_n\end{array}\right]$

易知J(q_o)为常数矩阵，式(4)可表示为

Δx＝J(q_o)Δq    (5)

这是一个6维的线性方程组，未知数的个数为n，机器人末端位置矢量增量Δx为已知量。

由上面的分析可知，机器人关节空间的运动规划问题就是Δq的求取问题，即求解式(5)所示的线性方程组。关于此方程组的求解分为几种情况：

1)dim(Δx)＝dim(Δq)，机器人关节个数等于机器人末端维度约束个数。

2)dim(Δx)＜dim(Δq)，机器人关节个数大于机器人末端维度约束个数。

3)dim(Δx)＞dim(Δq)，机器人关节个数小于机器人末端维度约束个数。

从信息融合的角度理解，可以把dim(Δx)看作是信息空间的维数，把dim(Δq)看作是决策空间的维数。当信息空间的维数等于决策空间的维数时(对应第一种情况)，信息可被直接利用，此方程组有唯一的算术解。当信息空间的维数小于决策空间的维数时(对应第二种情况)，需要把信息空间的信息统一投影到决策空间，使信息可直接为决策所利用。当信息空间的维数大于决策空间的维数时(对应第三种情况)，方程组无解。本发明主要针对第二种情况，当dim(Δx)＜dim(Δq)时，如何运用信息融合方法得到一个决策变量的最优解。

定理1设关于被估计量x∈Rⁿ的各种信息均可表示为

${\hat{y}}_i=H_{i}x+v_i,i=1~n.---\left(6\right)$

式中，为观测数据；为信息传递矩阵；为观测误差，且

$E\left[v_i\right]=0,E\left[v_i{v}_j^T\right]=\left\{\begin{array}{c}{R}_i,i=j\\ 0,i\≠j\end{array}\right.,$

若为非奇异，则是基于的最优融合估.且

$I\left[\hat{x}\right|x]=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{H}_i^T{R}_i^{-1}{H}_i---\left(7\right)$

$\hat{x}={\left\{I\right[\hat{x}\left|x\right]\}}^{-1}\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{H}_i^T{R}_i^{-1}{\hat{y}}_i---\left(8\right)$

式中，表示关于x的信息量.称式(8)为信息融合估计的统一线性模型.称为信息关于自身的信息量，记作 为关于被估计量x的信息量，记作所有关于被估计量x的信息的信息量之和等于最优融合估计关于自身的信息量，即

$I\left[\hat{x}\right|x]=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}I[{\hat{y}}_i\left|x\right]$

如(7)式所示.通常，信息关于自身的信息量与该信息的协方差互为倒数.

对于如式(5)所示的方程，由于dim(Δx)＜dim(Δq)，方程组(5)为关于未知量Δq的硬约束，为了求出Δq，还需要增加关于Δq的软约束，如下所示：

0＝Δq_i+ω_i，i＝1…n    (9)

软约束表示的意义，希望以最小的关节运动量实现机器人末端由点o运动到o′。ω_i表示每一个关节运动量的信息量，ω_i越大，表示越希望Δq_i趋近于0。综合式(5)、(9)可得如下增广方程组：

其中：m＝dim(Δx)；v_i→0，i＝1…n，为硬约束的信息量；n＝dim(Δq)，w_i的大小需要根据实际情况确定。

运用定理1可得如式(11)所示的解的表达式：

$\mathrm{\Δq}=\underset{v_i\→0}{\lim }\left\{{\left[H^T{R}^{-1}H\right]}^{-1}{H}^T{R}^{-1}\right[\frac{\mathrm{\Δx}}{0_{n\×1}}\left]\right\}---\left(11\right)$

其中：

$H=\left[\frac{J\left(q_0\right)}{I_{n\×n}}\right],$ $R=\left[\frac{\mathrm{diag}\left(v\right)}{\mathrm{diag}\left(w\right)}\right];$

由式(11)即可求出各个关节空间的运动量，通过选取不同的w_i值，可以调节各个关节运动量的大小。

具体实施方式

本发明旨在为机器人关节空间的运动规划设计一种新的方法，应当理解，本发明还可以应用在其它类似的多轴伺服控制系统中。

第一步：根据任务要求规划机器人末端的运动轨迹，确定末端位置和姿态。

第二步：根据机器人的模型求取机器人的雅可比矩阵。

第三步：将机器人的末端运动轨迹划分为若干等份，划分的等份数量越大，计算越精确，但计算量也越大；划分的等份数量越小，会导致计算误差增大。划分等份数应适当选取。

第四步：从机器人末端运动轨迹的起点开始，利用逆向运动学求出机器人各个关节在起点的位置初值，然后利用式(11)求取运动到下一步时，机器人各个关节的运动位置增量。

第五步：机器人关节的位置初值加上运动到下一步的运动位置增量，就可以得到机器人各个关节在下一步的位置。

第六步：以机器人关节下一步的位置作为新的位置初值，重复第四步和第五步的计算，直到机器人末端的终点位置。

第七步：保存每一步运动中机器人的关节位置，即可得到机器人关节的运动轨迹，从而完成了机器人关节空间的运动规划过程。

Claims (3)

1.一种新型的机器人关节空间运动规划方法，其特征在于：当机器人的末端需要进行轨迹跟踪控制，且机器人的运动关节个数大于机器人末端自由度约束个数时，通过对每个运动关节的运动量添加软约束，然后运用信息融合估计方法，可以规划出机器人关节空间的运动轨迹。

2.根据权利要求1所述的一种新型的机器人关节空间运动规划方法，其特征在于：所述的软约束是对运动关节期望运动量和相应信息量的描述方程。

3.根据权利要求1所述的一种新型的机器人关节空间运动规划方法，其特征在于：所述的信息融合估计方法是一种对包含信息量的线性矩阵方程的求解方法。