CN101244561A - 蒸汽发生器检修机械臂安装过程运动学逆解方法 - Google Patents

Abstract

本发明提供的是一种蒸汽发生器检修机械臂安装过程运动学逆解方法。其中的α、β、γ定义如下：初始时脚坐标系与世界坐标系姿态相同，然后脚坐标系绕自身y轴转α角，再绕自身x轴转β角，最后再绕自身y轴转γ角，得到最终的脚坐标系姿态。各关节点的位置分别为：B点为机械臂的术端点，C、D、E、F、G、H分别为第六关节至第一关节点，I为脚趾术端点，首先求出机械臂各个关节点的坐标，通过安装过程的正解解算出B，C，D的坐标；由I点的坐标和姿态角α，β，γ得出向量，然后可推出H点的坐标；求取F点坐标；其次求取各个关节的转角θ_i (i＝1，2，3，4，5，6)。本发明的方法避免了建立机械臂的运动学方程，大大减少了计算量，提高了运算的速度，而且能够求得全部解。

Description

蒸汽发生器检修机械臂安装过程运动学逆解方法

(一)技术领域

本发明涉及蒸汽发生器检修系统，具体涉及蒸汽发生器一次侧检修六自由度机械臂。

(二)背景技术

核电站的核反应堆冷却系统称为一回路系统，其主要功能是把反应堆正常运行时产生的热量传输给蒸汽发生器(简称SG)，将二回路侧的水加热并转化为驱动汽轮发电机组的饱和蒸汽。

蒸汽发生器是核动力装置中一、二回路之间的连接枢纽，也是核动力装置运行中发生故障最多的设备之一。蒸汽发生器的工作性能及安全可靠性对于整个系统的安全稳定运行是至关重要的。由于蒸汽发生器里面存在放射性物质，人不能进去进行维修，这时就需要机械臂代替人完成维修工作。

通常，在核电站大修时，需对蒸汽发生器进行必要的检修活动。在SG一回路侧，一般有传热管的涡流检查(ET)、传热管超声检查(UT)、传热管堵管、水室表面状况的电视检查，等等。大多数的检修活动是远距离操控进行的，例如，一个机械臂通过人孔安装于SG水室内，机械臂的前端携挂不同的检修工具即可实施不同的检修活动。一些国家已经研发出一些用于SG检修的机械手产品，并且在实际的SG检修工作中投入使用，例如韩国原子能研究院SG检修机械臂、印度核动力公司SG检修机械臂、法国Framatome公司SG检修机械臂等。但国内目前此项仍然空白。

机械臂运动学通常存在两类问题及即正运动学问题和逆运动学问题。前者是指在已知机械臂各关节角度变量的基础上，求解出机械臂末端执行器的位置矢量和姿态矢量的过程。逆运动学问题是指在给定机械臂末端执行器要到达的空间位姿时，找出得到该位姿的各个关节角度变量。位置逆解问题是机械臂机构学乃至机械臂运动学中的最基础也是最重要的研究问题之一，它直接关系到机械臂运动分析、离线编程、轨迹规划和实时控制等工作。因为速度和加速度分析都要在位置分析的基础之上才能进行，所以位置逆解问题是机械臂运动规划和轨迹规划的基础，只有通过运动学逆解把空间位姿转换为关节变量，才能实现对机械臂末端执行器的控制。而从工程应用的角度出发，位置逆解问题的研究成果可以很容易地应用到机械臂上面去。

求解机器人的运动学逆解问题实质上是求解超越函数的非线性方程，由于机器人本身的复杂性，要用一种通用的算法是很困难的。目前求解运动学逆解的方法主要有paul等人提出的代数法，Lee和Ziegler提出的几何法以及Pieper方法等。位置逆解的复杂程度往往与机械臂的结构有很大关系。由于一般情况下，六个自由度便可满足机械臂在工作空间内可达任一位姿，因此六自由度机械臂最具有研究价值和实用价值的。如果机械臂的结构尺寸有些特殊，如轴线平行或相交或轴线长度为零等情况下，逆解运算相对比较简单；而如果结构尺寸一般，且六个关节又都是转动副，则逆解运算较为困难，该问题被喻为是空间机构运动分析中的珠穆朗玛峰。无论是结构特殊还是一般，仅仅用某种方法求得六自由度机械臂的位置逆解还是不够的，还要在计算方法，计算精度等各个方面作进一步的研究。

(三)发明内容

本发明的目的在于提供一种简单易行，精度和速度高的蒸汽发生器检修机械臂安装过程运动学逆解方法。

本发明的目的是这样实现的：

其中的α、β、γ定义如下：初始时脚坐标系与世界坐标系姿态相同，然后脚坐标系绕自身y轴转α角，再绕自身x轴转β角，最后再绕自身y轴转γ角，得到最终的脚坐标系姿态。各关节点的位置分别为：B点为机械臂的末端点，C、D、E、F、G、H分别为第六关节至第一关节点，I为脚趾末端点，首先求出机械臂各个关节点的坐标，通过安装过程的正解解算出B，C，D的坐标；由I点的坐标和姿态角α，β，γ得出向量

然后可推出H点的坐标；求取F点坐标；其次求取各个关节的转角θ_i(i＝1，2，3，4，5，6)。

本发明还可以包含如下具体方法步骤：

1、所述的通过安装过程的正解解算出B，C，D的坐标的方法是：B，C，D的坐标通过安装过程的正解直接得到，

各点坐标变换关系如下：

其中(px，py，pz)为手末端点相对于坐标原点即世界坐标系原点的位置，45°，50°为机械臂推进装置轴线亦即第六关节轴线与挡板面和管板面的夹角，L1、L2分别为机械臂BC、CD关节间的长度，α₁为第六关节的转角，M1，M2，M3，M4，M5，M6为齐次矩阵，各齐次矩阵如下所示：

$M1=\left[\begin{array}{cccc}1& 0& 0& \mathrm{px}\\ 0& 1& 0& \mathrm{py}\\ 0& 0& 1& \mathrm{pz}\\ 0& 0& 0& 1\end{array}\right]$

$M4=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& -L0\\ 0& 0& 0& 1\end{array}\right]$

$M5=-\left[\begin{array}{cccc}{\mathrm{c\α}}_1& -s{\mathrm{\α}}_1& 0& 0\\ {\mathrm{s\α}}_1& {\mathrm{c\α}}_1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ $M6=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& -L1\\ 0& 0& 0& 1\end{array}\right]$

设O点齐次坐标为 $P:\left(\left[\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right]\right),$ 则有

B：M₁·M₂·M3·P

C：M₁·M₂·M₃·M₄·P

D：M₁·M₂·M₃·M₄·M₅·P。

2、所述的由I点的坐标和姿态角α，β，γ得出向量

然后可推出H点的坐标的方法是：

其中，L6为HI的长度，M1，M2，M3，M4，M5为齐次矩阵，各齐次矩阵的表达式如下：

$M1=\left[\begin{array}{cccc}1& 0& 0& \mathrm{Ix}\\ 0& 1& 0& \mathrm{Iy}\\ 0& 0& 1& \mathrm{Iz}\\ 0& 0& 0& 1\end{array}\right]$ $M2=\left[\begin{array}{cccc}\mathrm{c\α}& -\mathrm{s\α}& 0& 0\\ \mathrm{s\α}& \mathrm{c\α}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

$M_3=\left[\begin{array}{cccc}\mathrm{c\β}& 0& \mathrm{s\β}& 0\\ 0& 1& 0& 0\\ -\mathrm{s\β}& 0& \mathrm{c\β}& 0\\ 0& 0& 0& 1\end{array}\right]$ $M_4=\left[\begin{array}{cccc}\mathrm{c\γ}& -\mathrm{s\γ}& 0& 0\\ \mathrm{s\γ}& \mathrm{c\λ}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

$M_5=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& -L_6\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]ss$

设O点齐次坐标为 $P:\left(\left[\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right]\right),$ 则有

H：M₁·M₂·M₃·M₄·M₅·P。

3、所述的求取F点坐标的方法是：

F点在以D为球心以DF长为半径的球面上，由以D为球心的球与以H为圆心的圆相交就能得到F点的坐标，得到F点的坐标后沿着FG的方向即HI的方向平移FG长得到G点的坐标；或者也可以先求取G点的坐标，然后沿着GF方向平移GF长可以得到F点坐标；

球圆交点的求解过程如下：

由HI向量得到HI的姿态角，进而求得D点在以H点为坐标原点的坐标系下的坐标，建立新的坐标系，在这个坐标系下进行分析，用解析几何的方法求出期望解；

(1)设球心o₁在新的坐标系下的坐标为(x₀，y₀，z₀)。

旋转过程如下：

在已知世界坐标系下球心坐标

其中经过上述的转动和平移后便能解出球心o₂在新的坐标系下的坐标(x₀，y₀，z₀)；

已知球心的坐标为(x₀，y₀，z₀)，球心到两个交点的距离也是已知的，即球的半径ball_r，则解得球被圆所在的平面切割后的圆的半径r，也既是p₁o₃或p₂o₃，公式如下：

           p₁o₃ ²＝p₁o₁ ²-o₃o₁ ²＝ball_r²-y0²

根据求出的p₁o₃，以及x0，与以知的圆半径cir_r，可以判断解的情况；

④当|x0|＞|cir_r+p₁o₃|或|x0|＜|cir_r-p₁o₃|时，则圆与球无交点；

⑤当|x0|＝|cir_r+p₁o₃|时，则圆与球有唯一交点，此交点就是圆与X轴的交点(x0，0，0)，然后进行上述旋转与平移过程的逆过程即可解得交点在世界坐标系下的坐标；

⑥当|x0|＜|cir_r+p₁o₃|并且|x0|＞|cir_r-p₁o₃|时，则圆与球有两交点；具体求解过程如下：

根据坐标平移和旋转，解出球心在新的坐标系下的坐标，假设圆与球之间有两个交点，其分别为p₁，p₂，在三角形p₁o₁o₃中已知三条边长后，可以利用三角形余弦定理解得∠p₁o₁o₃，

p₁o₃ ²＝o₁p₁ ²+o₁o₃ ²+2o₁p₁×o₁o₃×cosθ＝x0²

＝cir_r²+o₁o₃ ²+2cir_r×o₁o₃×cosθ

利用上式解出的o₁p₁与X轴的一正一负两个夹角，可解p₁，p₂两点在新坐标系中的坐标，计算公式如下：

        xa＝o₁p₁×cosθ

解一：        ya＝0

        za＝o₁p₁×sinθ

        xb＝-o₁p₁×cosθ

解二：        yb＝0；

        zb＝-o₁p₁×sinθ

(2)、再通过刚开始的坐标平移和旋转的逆过程，也就是新坐标系向世界坐标系的转换过程解得p₁，p₂在世界坐标系中的坐标，也即是要解的两个解，步骤如下

到此，可以求解出F点的坐标。

4、所述的求取各个关节的转角θ_i(i＝1，2，3，4，5，6)的具体步骤为：

(1)求第五关节的转角θ₅

由与两个矢量的夹角可以得到，

C、D、F各点的坐标已经求得，则向量

可以直接求得，

两个矢量的夹角用余弦定理求得，或者将

两个矢量作为三角形的两边，以世界坐标系的原点作为三角形的一个顶点，以

作为三角形第三边，如此求得任意两边的夹角，再判断由

两个向量的差乘得到的向量与D点即第五关节点的电机轴初始法向量的方向是否一致，确定

夹角的正负号，相同取正否则取负；

(2)求第六关节的转角θ₆

由

矢量与

矢量差乘得到一个向量

即第五关节的电机轴向向量，见下式

$\stackrel{\→}{P}=\stackrel{\→}{\mathrm{CD}}\⊗\stackrel{\→}{\mathrm{DF}}$

再求

向量与D点初始法向量的夹角就是要求的θ₆，

求取

向量与D点初始法向量的夹角的方法同上，仍然要判断夹角的正负，即将

向量与D点初始法向量差乘，判断其与C点电机轴法向量方向是否一致，确定正负号；

(3)求第四关节的转角θ₄

与

差乘得到一个向量

即第三关节电机轴向向量，见下式

$\stackrel{\→}{P}=\stackrel{\→}{\mathrm{FG}}\⊗\stackrel{\→}{\mathrm{DF}}$

再求

向量与F点初始法向量的夹角即可以求得第四关节的转角θ₄；

具体的求解方法与θ₆的求解过程相同；

(4)求第三关节的转角θ₃

由

与E点的法向量的夹角就可以求得，

具体求解与θ₅的求解过程相同；

(5)求第二关节的转角θ₂

由F点的法向量与矢量的夹角可以求得；

(6)求第一关节的转角θ₁。

本发明根据该蒸汽发生器检修机械臂(包括推进装置、机械臂各关节、检修工具以及检修水室等)的几何和机械构造，提供了一种简单易行，运算精度和速度较高的机械臂安装过程反解的计算方法。

根据该检修机械臂的具体结构特点，给出了一种简单易行的几何解法，就是设法把机械臂的空间几何问题分解成若干个平面或空间解析几何问题，在不联立求解多个含有未知数的运动学方程的情况下，就可以求出关节位置和角度。

本发明利用坐标的平移旋转变换、向量及向量之间的计算、以及求取球圆交点等方法可以求取点的坐标以及关节转角，即将关节点的位姿求解用平面、空间解析几何方法细化，逐步求取预知信息。

机械臂的运动学逆解问题即已知_n ⁰T(工具坐标系相对于世界坐标系的位姿)，求出θ₁，θ₂，…，θ_n(各个关节的转角)的值。只要_n ⁰T表示的末端连杆坐标系的位置和姿态位于机械臂的可达工作空间内，则运动学方程至少有一个解存在。但是可达空间内，有一部分是灵活工作空间，即机械臂的末端不但可以达到，而且具有任意姿态。因此，运动学方程有可能出现重解，一般地说，不为零的连杆参数愈多，解的个数愈多。

机械臂运动学方程是一组非线性方程式，没有通用的解法。如果采用数值迭代程，求解过程显得很慢，而且在重解的情况下，也不能保证求出所有的解，因此，一般都希望采用各种技巧，求出运动学方程的解的解析表达式，即封闭形式的解。

本发明的方法避免了建立机械臂的运动学方程，大大减少了计算量，提高了运算的速度，而且能够求得全部解。针对该机械臂的具体情况，为了简化问题规模并提高求解速度和质量，提出一种代数法和数值优化方法结合的办法，保证解的优化过程沿着精确解集进行，使得逆解精度不受优化精度的影响，并且通过数学变换，成功地将多目标多参数优化问题转化为单目标单参数优化问题，使得优化过程简单快速，更加有效地解决了该机械臂的运动学逆解问题。并提出一种基于非单调信赖域的优化算法，该算法不仅具有拟牛顿法的收敛速率，而且有理想的总体收敛特性，该算法不但稳定，而且简单，执行效率快，可用于在线求解。

(四)附图说明

图1是机械臂各臂总图；

图2是机械臂安装过程各关节点的标架图；

图3是机械臂安装过程任意姿态的状态图；

图4是球圆交点求解原理示意图。

(五)具体实施方式

下面结合附图举例对本发明做更详细地描述：

本算法中的α、β、γ定义如下：初始时脚坐标系与世界坐标系姿态相同，然后脚坐标系绕自身y轴转α角，再绕自身x轴转β角，最后再绕自身y轴转γ角，得到最终的脚坐标系姿态。

附图1是机械臂各臂总图，各关节点的位置已经在图中标示出来(该逆解算法中的各关节点均按照图中标示位置计算)，其中B点为机械臂的末端点，C、D、E、F、G、H分别为第六关节至第一关节点，I为脚趾末端点。

本蒸汽发生器检修机械臂运动学逆解算法是在I点的期望位置、当前脚坐标系的姿态角和当前各关节角度基础上，求解期望的各关节点的转角，也即在解算下一目标点位置和转角的时候，上一目标点的位置和转角是已知的。该机械臂安装过程运动学逆解算法的实现过程分为以下两步：

第一，首先求出机械臂各个关节点的坐标：

该机械臂安装过程运动学反解算法中坐标平移旋转变换依照附图2所示标架图的坐标系进行，图中表示的并非实际位置，只是表示了各关节点的坐标轴方向。

1、通过安装过程的正解解算出B，C，D的坐标

B，C，D的坐标通过安装过程的正解可以直接得到。

各点坐标变换关系如下：

其中(px，py，pz)为手末端点相对于坐标原点即世界坐标系原点的位置，45°，50°为机械臂推进装置轴线亦即第六关节轴线(BC点连线所在直线)与挡板面和管板面的夹角。L1、L2分别为机械臂BC、CD关节间的长度(即臂长)，α₁为第六关节的转角，M1，M2，M3，M4，M5，M6为齐次矩阵，各齐次矩阵如下所示：

$M1=\left[\begin{array}{cccc}1& 0& 0& \mathrm{px}\\ 0& 1& 0& \mathrm{py}\\ 0& 0& 1& \mathrm{pz}\\ 0& 0& 0& 1\end{array}\right]$

$M4=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& -L0\\ 0& 0& 0& 1\end{array}\right]$

$M5=-\left[\begin{array}{cccc}{\mathrm{c\α}}_1& -s{\mathrm{\α}}_1& 0& 0\\ {\mathrm{s\α}}_1& {\mathrm{c\α}}_1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ $M6=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& -L1\\ 0& 0& 0& 1\end{array}\right]$

设O点(世界坐标系原点)齐次坐标为，则有 $P:\left(\left[\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right]\right)$

B：M₁·M₂·M3·P

C：M₁·M₂·M₃·M₄·P

D：M₁·M₂·M₃·M₄·M₅·P

2、由I点的坐标和姿态角α，β，γ(I点的位姿已知)得出向量

然后可推出H点的坐标。

其中，L6为HI的长度，M1，M2，M3，M4，M5为齐次矩阵，各齐次矩阵的表达式如下：

$M1=\left[\begin{array}{cccc}1& 0& 0& \mathrm{Ix}\\ 0& 1& 0& \mathrm{Iy}\\ 0& 0& 1& \mathrm{Iz}\\ 0& 0& 0& 1\end{array}\right]$ $M2=\left[\begin{array}{cccc}\mathrm{c\α}& -\mathrm{s\α}& 0& 0\\ \mathrm{s\α}& \mathrm{c\α}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

$M_3=\left[\begin{array}{cccc}\mathrm{c\β}& 0& \mathrm{s\β}& 0\\ 0& 1& 0& 0\\ -\mathrm{s\β}& 0& \mathrm{c\β}& 0\\ 0& 0& 0& 1\end{array}\right]$ $M_4=\left[\begin{array}{cccc}\mathrm{c\γ}& -\mathrm{s\γ}& 0& 0\\ \mathrm{s\γ}& \mathrm{c\λ}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

$M_5=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& -L_6\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

设O点(世界坐标系的原点)齐次坐标为 $P:\left(\left[\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right]\right),$ 则有

H：M₁·M₂·M₃·M₄·M₅·P

3、F点在以D为球心以DF长为半径的球面上。由于HI的姿态固定所以G是以H为圆心以GH为半径的圆，同时由于FG的方向和HI保持一致，而且F与G只相差一个长度(FG的长)，所以F也是一个圆。由以D为球心的球与以H为圆心的圆相交就能得到F点的坐标，得到F点的坐标后很容易就可以求得G点的坐标(只要沿着FG的方向即HI的方向平移FG长便可以得到)。

或者，也可以先求取G点的坐标，然后沿着GF方向平移GF长可以得到F点坐标。G是以H为圆心以GH为半径的圆，将D点沿着FG方向平移FG长，再以此时的D点为球心，以DF长为半径作球，两者的交点即为期望的G点坐标。E点坐标对于求解关节的转角没有意义，故不必求解。

球圆交点的求解过程如下：

由HI向量可以得到HI的姿态角，进而可以求得D点在以H点为坐标原点的坐标系下的坐标，如此建立新的坐标系，在这个坐标系下进行分析，用解析几何的方法可以求出期望解。求解球圆交点的原理图如附图4所示。

(1)设球心o₁在新的坐标系下的坐标为(x₀，y₀，z₀)。

旋转过程如下：

在已知世界坐标系下球心坐标

其中经过上述的转动和平移后便能解出球心o₂在新的坐标系下的坐标(x₀，y₀，z₀)。

已知球心的坐标为(x₀，y₀，z₀)，球心到两个交点的距离也是已知的，即球的半径ball_r，则我们可解得球被圆所在的平面切割后的圆的半径r，也既是p₁o₃或p₂o₃，公式如下：

            p₁o₃ ²＝p₁o₁ ²-o₃o₁ ²＝ball_r²-y0²

根据求出的p₁o₃，以及x0，与以知的圆半径cir_r，可以判断解的情况。

⑦当|x0|＞|cir_r+p₁o₃|或|x0|＜|cir_r-p₁o₃|时，则圆与球无交点。

⑧当|x0|＝|cir_r+p₁o₃|时，则圆与球有唯一交点。此交点就是圆与X轴的交点(x0，0，0)，然后进行上述旋转与平移过程的逆过程即可解得交点在世界坐标系下的坐标。

⑨当|x0|＜|cir_r+p₁o₃|并且|x0|＞|cir_r-p₁o₃|时，则圆与球有两交点。具体求解过程如下：

根据坐标平移和旋转，可解出球心在新的坐标系下的坐标，我们现在假设圆与球之间有两个交点，其分别为p₁，p₂。如附图4所示：在三角形p₁o₁o₃中已知三条边长后，可以利用三角形余弦定理解得∠p₁o₁o₃，

p₁o₃ ²＝o₁p₁ ²+o₁o₃ ²+2o₁p₁×o₁o₃×cosθ＝x0²

＝cir_r²+o₁o₃ ²+2cir_r×o₁o₃×cosθ

利用我们上式解出的o₁p₁与X轴的两个夹角(一正一负)，可解p₁，p₂两点存新坐标系中的坐标，计算公式如下：

        xa＝o₁p₁×cosθ

解一：        ya＝0

        za＝o₁p₁×sinθ

        xb＝-o₁p₁×cosθ

解二：        yb＝0

        zb＝-o₁p₁×sinθ

(2)、再通过刚开始的坐标平移和旋转的逆过程，也就是新坐标系向世界坐标系的转换过程解得p₁，p₂在世界坐标系中的坐标，也即是我们要解的两个解，步骤如下

到此，可以求解出F点的坐标。

第二，求取各个关节的转角θ_i(i＝1，2，3，4，5，6)：

1、求第五关节的转角θ₅

由与两个矢量的夹角可以得到。

C、D、F各点的坐标已经求得，则向量

可以直接求得。

两个矢量的夹角可用余弦定理求得，或者将

两个矢量作为三角形的两边，以世界坐标系的原点作为三角形的一个顶点，以

作为三角形第三边，如此可以求得任意两边的夹角，再判断由

两个向量的差乘得到的向量与D点即第五关节点的电机轴初始法向量的方向是否一致，确定

夹角的正负号，相同取正否则取负。为了增加函数的通用性，利于编程实现，本算法采用后面的方法。

2、求第六关节的转角θ₆

由

矢量与矢量差乘得到一个向量

即第五关节的电机轴向向量，见下式

$\stackrel{\→}{P}=\stackrel{\→}{\mathrm{CD}}\⊗\stackrel{\→}{\mathrm{DF}}$

再求

向量与D点初始(零位)法向量的夹角就是要求的θ₆。

求取向量与D点初始(零位)法向量的夹角的方法同上，仍然要判断夹角的正负，即将向量与D点初始(零位)法向量差乘，判断其与C点电机轴法向量方向是否一致，确定正负号。

注意：当BCD三点共线时，θ₆可以不求，θ₄取到的作用可以代替θ₆，此时θ₆的角度保持当前值。

3、求第四关节的转角θ₄

与差乘得到一个向量

即第三关节电机轴向向量，见下式

$\stackrel{\→}{P}=\stackrel{\→}{\mathrm{FG}}\⊗\stackrel{\→}{\mathrm{DF}}$

再球

向量与F点初始法向量的夹角即可以求得第四关节的转角θ₄。

具体的求解方法参照θ₆的求解过程。

注意：如果DFG三点共线时，θ₄可以不求，θ₄取到的作用可用θ₂来代替，即此时θ₄的角度保持当前值。

4、求第三关节的转角θ₃

由

与E点的法向量的夹角就可以求得。

具体求解参照θ₅的求解过程。

5、求第二关节的转角θ₂

由F点的法向量与

矢量的夹角可以求得。

求解方法同上。

6、求第一关节的转角θ₁

由上面α，β，γ的定义可知，θ₁的值由γ值的变化所决定，即θ₁等于γ的变化量。

以上涉及到的法线向量(电机轴的轴线方向)的求解，通过运动学的正解，再沿其法线方向上平移一个单位，将平移后的坐标与平移前的坐标做差，就可以得出对应关节点的法线方向向量。

注：各关节点的法线方向向量都在坐标轴上。

Claims (5)

1、一种蒸汽发生器检修机械臂安装过程运动学逆解方法，其中的α、β、γ定义如下：初始时脚坐标系与世界坐标系姿态相同，然后脚坐标系绕自身y轴转α角，再绕自身x轴转β角，最后再绕自身y轴转γ角，得到最终的脚坐标系姿态；各关节点的位置分别为：B点为机械臂的末端点，C、D、E、F、G、H分别为第六关节至第一关节点，I为脚趾末端点，其特征是：首先求出机械臂各个关节点的坐标，通过安装过程的正解解算出B，C，D的坐标；由I点的坐标和姿态角α，β，γ得出向量

然后可推出H点的坐标；求取F点坐标；其次求取各个关节的转角θ_i(i＝1，2，3，4，5，6)。

2、根据权利要求1所述的蒸汽发生器检修机械臂安装过程运动学逆解方法，其特征是：所述的通过安装过程的正解解算出B，C，D的坐标的方法是：B，C，D的坐标通过安装过程的正解直接得到，

各点坐标变换关系如下：

其中(px，py，pz)为手末端点相对于坐标原点即世界坐标系原点的位置，45°，50°为机械臂推进装置轴线亦即第六关节轴线与挡板面和管板面的夹角，L1、L2分别为机械臂BC、CD关节间的长度，α₁为第六关节的转角，M1，M2，M3，M4，M5，M6为齐次矩阵，各齐次矩阵如下所示：

$M1=\left[\begin{array}{cccc}1& 0& 0& \mathrm{px}\\ 0& 1& 0& \mathrm{py}\\ 0& 0& 1& \mathrm{pz}\\ 0& 0& 0& 1\end{array}\right]$

$M4=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& -L0\\ 0& 0& 0& 1\end{array}\right]$

$M5=-\left[\begin{array}{cccc}{\mathrm{c\α}}_1& -s{\mathrm{\α}}_1& 0& 0\\ {\mathrm{s\α}}_1& {\mathrm{c\α}}_1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$ $M6=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& -L1\\ 0& 0& 0& 1\end{array}\right]$

设O点齐次坐标为 $P:\left(\left[\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right]\right),$ 则有

B：M₁·M₂·M3·P

C：M₁·M₂·M₃·M₄·P

D：M₁·M₂·M₃·M₄·M₅·P。

3、根据权利要求2所述的蒸汽发生器检修机械臂安装过程运动学逆解方法，其特征是：所述的由I点的坐标和姿态角α，β，γ得出向量

然后可推出H点的坐标的方法是：

其中，L6为HI的长度，M1，M2，M3，M4，M5为齐次矩阵，各齐次矩阵的表达式如下：

$M1=\left[\begin{array}{cccc}1& 0& 0& \mathrm{Ix}\\ 0& 1& 0& \mathrm{Iy}\\ 0& 0& 1& \mathrm{Iz}\\ 0& 0& 0& 1\end{array}\right]$ $M2=\left[\begin{array}{cccc}\mathrm{c\α}& -\mathrm{s\α}& 0& 0\\ \mathrm{s\α}& \mathrm{c\α}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

$M_3=\left[\begin{array}{cccc}\mathrm{c\β}& 0& \mathrm{s\β}& 0\\ 0& 1& 0& 0\\ -\mathrm{s\β}& 0& \mathrm{c\β}& 0\\ 0& 0& 0& 1\end{array}\right]$ $M_4=\left[\begin{array}{cccc}\mathrm{c\γ}& -\mathrm{s\γ}& 0& 0\\ \mathrm{s\γ}& \mathrm{c\λ}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

$M_5=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& -L_6\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

设O点齐次坐标为 $P:\left(\left[\begin{array}{c}0\\ 0\\ 0\\ 1\end{array}\right]\right),$ 则有

H：M₁·M₂·M₃·M₄·M₅·P。

4、根据权利要求3所述的蒸汽发生器检修机械臂安装过程运动学逆解方法，其特征是：所述的求取F点坐标的方法是：

F点在以D为球心以DF长为半径的球面上，由以D为球心的球与以H为圆心的圆相交就能得到F点的坐标，得到F点的坐标后沿着FG的方向即HI的方向平移FG长得到G点的坐标；或者也可以先求取G点的坐标，然后沿着GF方向平移GF长可以得到F点坐标；

球圆交点的求解过程如下：

由HI向量得到HI的姿态角，进而求得D点在以H点为坐标原点的坐标系下的坐标，建立新的坐标系，在这个坐标系下进行分析，用解析几何的方法求出期望解；

(1)设球心o₁在新的坐标系下的坐标为(z₀，y₀，z₀)。

旋转过程如下：

在已知世界坐标系下球心坐标

其中经过上述的转动和平移后便能解出球心o₂在新的坐标系下的坐标(x₀，y₀，z₀)；

已知球心的坐标为(x₀，y₀，z₀)，球心到两个交点的距离也是已知的，即球的半径ball_r，则解得球被圆所在的平面切割后的圆的半径r，也既是p₁o₃或p₂o₃，公式如下：

               p₁o₃ ²＝p₁o₁ ²-o₃o₁ ²＝ball_r²-y0²

根据求出的p₁o₃，以及x0，与以知的圆半径cir_r，可以判断解的情况；

①当|x0|＞|cir_r+p₁o₃|或|x0|＜|cir_r-p₁o₃|时，则圆与球无交点；

②当|x0|＝|cir_r+p₁o₃|时，则圆与球有唯一交点，此交点就是圆与X轴的交点(x0，0，0)，然后进行上述旋转与平移过程的逆过程即可解得交点在世界坐标系下的坐标；

③当|x0|＜|cir_r+p₁o₃|并且|x0|＞|cir_r-p₁o₃|时，则圆与球有两交点；具体求解过程如下：

根据坐标平移和旋转，解出球心在新的坐标系下的坐标，假设圆与球之间有两个交点，其分别为p₁，p₂，在三角形p₁o₁o₃中已知三条边长后，可以利用三角形余弦定理解得∠p₁o₁o₃，

p₁o₃ ²＝o₁p₁ ²+o₁o₃ ²+2o₁p₁×o₁o₃×cosθ＝x0²

＝cir_r²+o₁o₃ ²+2cir_r×o₁o₃×cosθ

利用上式解出的o₁p₁与X轴的一正一负两个夹角，可解p₁，p₂两点在新坐标系中的坐标，计算公式如下：

          xa＝o₁p₁×cosθ

解一：        ya＝0

          za＝o₁p₁×sinθ

          xb＝-o₁p₁×cosθ

解二：        yb＝0；

          zb＝-o₁p₁×sinθ

(2)、再通过刚开始的坐标平移和旋转的逆过程，也就是新坐标系向世界坐标系的转换过程解得p₁，p₂在世界坐标系中的坐标，也即是要解的两个解，步骤如下

到此，可以求解出F点的坐标。

5、根据权利要求4所述的蒸汽发生器检修机械臂安装过程运动学逆解方法，其特征是：所述的求取各个关节的转角θ_i(i＝1，2，3，4，5，6)的具体步骤为：

(1)求第五关节的转角θ₅

由

与

两个矢量的夹角可以得到，

C、D、F各点的坐标已经求得，则向量

可以自接求得，

两个矢量的夹角用余弦定理求得，或者将

两个矢量作为三角形的两边，以世界坐标系的原点作为三角形的一个顶点，以

作为三角形第三边，如此求得任意两边的夹角，再判断由

两个向量的差乘得到的向量与D点即第五关节点的电机轴初始法向量的方向是否一致，确定

夹角的正负号，相同取正否则取负；

(2)求第六关节的转角θ₆

由矢量与

矢量差乘得到一个向量

即第五关节的电机轴向向量，见下式

$\stackrel{\→}{P}=\stackrel{\→}{\mathrm{CD}}\⊗\stackrel{\→}{\mathrm{DF}}$

再求向量与D点初始法向量的夹角就是要求的θ₆，

求取

向量与D点初始法向量的夹角的方法同上，仍然要判断夹角的正负，即将

向量与D点初始法向量差乘，判断其与C点电机轴法向量方向是否一致，确定正负号；

(3)求第四关节的转角θ₄

与

差乘得到一个向量即第三关节电机轴向向量，见下式

$\stackrel{\→}{P}=\stackrel{\→}{\mathrm{FG}}\⊗\stackrel{\→}{\mathrm{DF}}$

再求

向量与F点初始法向量的夹角即可以求得第四关节的转角θ₄；

具体的求解方法与θ₆的求解过程相同；

(4)求第三关节的转角θ₃

由

与E点的法向量的夹角就可以求得，

具体求解与θ₅的求解过程相同；

(5)求第二关节的转角θ₂

由F点的法向量与

矢量的夹角可以求得；

(6)求第一关节的转角θ₁。

Images