CN101745916B - 带容错和纠错功能的并联机器人装置及其容错和纠错方法 - Google Patents

Abstract

该发明属于一种带容错和纠错功能的并联机器人装置及容错和纠错方法。装置包括动平台、固定平台及其驱动腿，串接有位移传感器和张紧弹簧的柔索，传动系统，含FPGA的控制系统；而容错和纠错方法包括状态监测与故障诊断、故障定位及故障传感器的容错和纠错。该发明由于在动平台与固定平台中心之间增设了带传感器和张紧弹簧的柔索，并在控制系统中采用FPGA控制器；因而具有对驱动腿传感器故障可进行在线重构和自维修，且重构时间短，速度快，系统的重构和自维修效率高，可确保机器人在驱动腿传感器发生故障时能正常工作，从而有效提高了并联机器人的工作效率及工作和运行的安全性、可靠性，可避免因驱动腿传感器突发性故障造成重大损失等特点。

Description

带容错和纠错功能的并联机器人装置及其容错和纠错方法

技术领域

本发明属于机器人制造技术领域，特别是一种带容错和纠错功能的四自由度并联机器人装置及针对该项机器人传感器输入故障的容错和纠错方法。

背景技术

要实现对并联机器人位置、速度的精确控制，就必须通过高精度的传感器对这些物理量进行实时检测，并将检测结果转换成数字量，反馈给控制器，通过控制器对这些数据进行处理，处理的结果作为控制量对伺服电机进行控制，进而对机器人的运动等实现精确的闭环控制。所以检测用传感器加上反馈环节是伺服电机闭环控制系统的重要组成部分。在申请号为00105935.1的专利文献中，公开了《一种四自由度并联机器人机构》，该并联机器人的动平台通过四个分支(驱动腿)与固定平台连接，形成并联闭环结构，其中两个分支各含有两个单自由度运动副，一个球铰链；另外两个分支各含有一个单自由度运动副，两个球铰链或一个球铰链和一个虎克铰；此类并联机器人的位置、速度(运动)依靠设于各分支(条腿)上的含驱动装置(伺服电机)、带光电编码器的传感器在内的传动系统及PID(比例积分微分)控制器相互配合来实现。PID(比例积分微分)控制器根据位置、速度传感器的检测信息形成对位置、速度等的闭环控制。该发明虽然具有运动正反解容易、高刚度、高精度、低运动质量、高动态性能、结构简单等优点；但由于PID控制器本身不具备容错、数据重构等功能；因而存在缺乏在线自我修复的能力，当机器人在运动过程中一旦传感器发生故障、就将导致传感器无输出或输出结果不正确，轻则造成位置、速度控制的精度降低，重则造成并联机器人的失控、导致严重的安全事故等缺陷。

发明内容

本发明的目的是针对背景技术存在的缺陷，研究设计一种带容错和纠错功能的并联机器人装置及其容错和纠错方法，以便在驱动腿传感器出现突发性故障时，通过系统内的切换和重构实现容错及自动纠错、进行在线自维修，确保其正常工作和运行，从而达到有效提高并联机器人运行的安全性和可靠性，有效避免因传感器突发性故障造成重大损失等目的。

本发明的解决方案是在背景技术四自由度2RPS+2TPS型并联机器人装置(机构)的基础上，在并联机器人运动平台中心与固定平台的中心之间增设一条柔索，柔索的一端串接一位移传感器及使柔索保持张紧状态的弹簧，在工作过程中，将由四条驱动腿的传感器测量值与柔索传感器的测量值送入故障检测与隔离单元，用于对并联机器人的运行状态进行监控；当四条驱动腿传感器中有一传感器出现了故障，判断出故障传感器后，则利用空间闭链机构约束，由其余三条驱动腿和柔索上传感器的检测值、通过现场可编程门阵列(FPGA)逻辑控制器中的数据重构单元重新计算得到该故障传感器的应测值，将该应测值替代故障传感器输出的错误值，而将故障传感器的原检测(异常)值舍弃，从而实现对传感器故障的容错和自动纠错，确保机器人正常工作。因此，本发明并联机器人装置包括动平台、固定平台(机架)及将两平台活动连接以形成并联闭环结构的四条驱动腿(分支)，其中两个驱动腿各含有两个单自由度运动副、一个球铰链，另外两个驱动腿各含有一个单自由度运动副和两个球铰链或一个球铰链和一个虎克铰，设于各驱动腿上的含传感器、伺服驱动器、伺服电机在内的传动系统，以及与各传动系统连接以控制机器人运动的控制系统，关键在于在动平台中心与固定平台的中心之间还设有一条柔索，在与固定平台连接点外侧的该柔索上、还串接有用于监测两中心之间距离变化的位移传感器和使柔索呈张紧状态的弹簧，而控制系统则为含现场可编程门阵列(FPGA)逻辑控制器的控制系统；柔索的一端与动平台中心固定，另一端则与固定平台的中心可相对滑动式连接、并在连接点外侧的柔索上串接位移传感器和张紧弹簧后再紧固于固定平台上，该位移传感器还通过信号线与控制器连接、以传送监测信息。

以上所述含现场可编程门阵列(FPGA)逻辑控制器的控制系统，其控制系统包括闪存器(FLASH)，随机存取存储器(RAM)及与之连接的含中心控制单元、用于与PC机连接的通讯单元、用于插补计算的插补运算单元、用作PID控制的PID控制单元、用于处理反馈信号的计数单元、用于将反馈参数与输入闪存器中对应的运行参数进行比较的故障检测与隔离单元、将该故障检测值进行隔离处理的缓冲单元、以及用于计算故障传感器应测值的数据重构单元在内的现场可编程门阵列(FPGA)逻辑控制器。而所述柔索为在长度方向的弹性变形量足够小的金属或非金属类柔索；而与固定平台中心的连接则通过环扣或环勾作可相对滑动式连接。

而本发明并联机器人装置工作过程中的容错和纠错方法，包括：

A.状态监测与故障诊断：首先根据并联机器人的运动要求，通过轨迹规划，将其各条驱动腿长度、速度随时间变化的运行参数输入控制系统的闪存器(FLASH)内，作为驱动腿长度、速度的基础参数，该参数值同时经逻辑控制器中的插补运算单元处理后将结果送入PID控制单元用于对各驱动腿内的伺服电机进行控制；四条驱动腿传感器及柔索传感器的在线检测值一路经缓冲单元输入PID控制单元对伺服电机进行PID控制，另一路在中心控制单元的控制下作为检测值输入随机存取存储器(RAM)、以对并联机器人的运行状况进行在线控制和监测；由四条驱动腿传感器检测值根据空间并联机构的约束关系可以计算出中间柔索传感器的计算值，并将此柔索传感器的计算值与柔索传感器的实际检测值比较，如果两者之间的偏差值超出了设定范围，则表明有一条驱动腿的传感器出现了故障；

B.故障定位：故障检测与隔离系统将驱动腿传感器此时的基础参数与驱动腿传感器的实际输出值进行对比，如果某一驱动腿两者之间的偏差值超出了设定范围，则判定该驱动腿传感器为故障传感器；

C.故障传感器的容错和纠错：当由B步骤确定出故障传感器后，即由另外三条驱动腿传感器的检测值和中间柔索传感器的检测值通过FPGA运动控制器中的数据重构单元，计算出故障传感器的应测值，并在检测与隔离单元的控制下通过缓冲器将故障检测值丢弃，以该应测值替换，用于对故障传感器所对应的伺服电机进行控制，从而实现对故障传感器的容错和纠错。

所述故障传感器包括驱动腿传感器本身故障或其电路传输故障、或传感器探头因物理障碍所致异常输出。

本发明带容错和纠错功能的并联机器人装置由于在传统四自由度2RPS+2TPS型并联机器人装置的运动平台中心与固定平台的中心之间连接一条柔索，柔索一端串接一位移传感器及使柔索保持张紧状态的弹簧，柔索对于并联机器人的运动没有干涉，其上连接的传感器所提供的冗余信息在并联机器人正常工作时被用于故障检测与隔离；其容错和纠错方法则是在控制系统中采用具有并行计算能力和可动态重构特点的现场可编程门阵列逻辑(FPGA)控制器，对并联机器人运行进行动态监测；当并联机器人驱动腿传感器出现故障时，则利用其余正常工作传感器的测量值、通过其内部逻辑资源的重新配置自动进行在线数据重构，以确保系统正常运行和修复。因而，本发明机器人装置在工作过程中当驱动腿传感器发生突发性故障时，可进行在线自维修，对传感器故障容错并自动纠错，且重构时间短，速度快，系统的重构效率高，确保机器人在驱动腿的传感器发生故障时正常工作并及时修复，从而有效提高了并联机器人的工作效率及工作和运行的安全性、可靠性，可有效避免因传感器突发性故障造成重大损失等特点。克服了背景技术存在的如果传感器一旦发生故障，将导致传感器无输出或输出结果不正确，轻则造成位置、速度控制的精度降低，重则造成并联机器人的失控、甚至导致严重事故的发生等弊病。

附图说明

图1.为本发明并联机器人装置结构示意图；

图2.为本发明并联机器人装置控制系统及与PC机和各驱动腿连接关系示意图；

图3.为本发明实施方式与图1中A₁B₁B₃A₃环路的运动等效的平面并联机构示意图。

图中：1.动平台，2.固定平台、2-1.环扣，3.驱动腿，3-1(l₁)、3-2(l₂)、3-3(l₃)、3-4(l₄)：(分别为第1、2、3、4驱动腿)，4(l₅).柔索、4-1.位移传感器、4-2.张紧弹簧，5(O₁P).指针，6.闪存器(FLASH)，7.随机存取存储器(RAM)，8.现场可编程门阵列(FPGA)逻辑控制器，9.PC机，I.数模转换器、II.伺服驱动器、III.伺服电机、IV.驱动腿传感器；A₁、A₃：转动副，A₂、A₄：虎克铰或球铰，B₁、B₂、B₃、B₄：球铰。

具体实施方式

本实施方式的控制系统中：FPGA逻辑控制器8采用型号为SPARTAN-3的FPGA逻辑器件并通过内部逻辑资源的重新配置，组成包含中心控制单元、通讯单元、插补运算单元、PID控制单元、数据重构单元、故障检测与隔离单元、计数单元、缓冲单元在内的控制器；闪储器(FLASH)6型号为Am29LV160D，随机存取存储器(RAM)7采用型号为HY57V641620HG的同步动态随机存取存储器，数模转换器I型号为AD7564，伺服电机III型号为MSMA042A1G、与之配套的伺服驱动器II型号为MSDA043A1A，驱动腿传感器IV采用型号为CN65M121759的光电编码器。

本实施方式对传感器故障的容错和自动纠错方法，以在并联机器人仿真运行为例：并联机器人结构参数如下：固定平台2和动平台1都为正方形布局，其外接圆的半径分别为R＝175mm，r＝100mm，动平台1中心O₁点到固定平台(A₁、A₂、A₃、A₄)的距离h＝350mm，在动平台1的中心固定有一指针5(O₁P)，指针垂直于动平台1，指针5(O₁P)长为100mm；柔索4(l₅)选用的是钢丝绳，钢丝绳通过圆环2-1改变了走向，钢丝绳另一端连接型号为CN63M/GXC的光栅尺传感器及弹簧4-2，弹簧4-2使柔索4(l₅)始终处于张紧状态；规划指针5(O₁P)绕固定平台2的Z轴做匀速圆周运动，O₁P与Z轴的夹角θ＝45°，运动周期为360秒。

其方法如下：

A.首先根据上述运动要求，由PC机9通过轨迹规划将各条驱动腿长度随时间变化的运行参数输入控制系统的闪存器6，作为驱动腿长度的基础参数同时交由插补运算单元处理后送入PID控制单元，其输出控制信号经数摸转换器I转换后通过伺服驱动器II控制伺服电机III的运行；由各传感器IV检测所得的反馈值，输入计数单元处理后，送入PID控制单元用于对伺服电机III的反馈控制，同时，该反馈数据被存入随机存取存储器7中；

B.在t＝115秒时，在传感器IV引入突发性误差(如传感器的输出值突然消失)，故障检测与隔离单元由四条驱动腿长度的检测值、并根据空间并联机构的约束关系计算出柔索4(l₅)位于固定平台2和动平台1中心之间长度的计算值，将该段柔索长度的计算值与该段柔索长度的实际检测值比较，如果偏差值超出了设定范围，则表明有一条驱动腿的传感器出现了故障；故障检测与隔离系统将此时刻闪存器6中的驱动腿长度的基础参数与随机存取存储器7中的反馈的驱动腿传感器的实际输出值进行对比，此时由于驱动腿3-4(l₄)两者之间的偏差超出了设定范围，故判定驱动腿3-4(l₄)传感器为故障传感器；

C.在由B步骤检测并定位出驱动腿3-4(l₄)传感器出现故障时，即通过故障检测与隔离单元向缓存单元发出隔离指令，同时，中心控制单元启动数据重构单元进行数据重构处理，本实施方式数据重构单元按下述方法自动进行数据重构：

建立坐标系如图1所示，坐标系设定如下：基础坐标系R的坐标原点位于固定平台的中心O点，X轴选取为OA₃方向，Z轴选取为垂直于平面A₁A₂A₃A₄的方向；动坐标系R’的坐标原点位于动平台的中心O₁点，X’轴选取为O₁B₃方向，Z’轴选取为垂直于平面B₁B₂B₃B₄的方向。

图1所示并联机器人中A₁B₁B₃A₃环路的运动与图3所示的平面并联机构的运动等效，动平台上铰接点B₁、B₃在基础坐标系R中的坐标可表示为：

B₁＝(l₅cosθ₁-rcosθ₂，0，l₅sinθ₁-rsinθ₂)^T(1)

B₃＝(l₅cosθ₁+rcosθ₂，0，l₅sinθ₁+rsinθ₂)^T

固定平台上铰接点A₁、A₃在基础坐标系R中的坐标可表示为：

A₁＝(-R，0，0)^T，A₃＝(R，0，0)^T    (2)

驱动腿的长度l_i可以用铰链点A_i、B_i的坐标表示出来，即：

$\left\{\begin{array}{c}{(l_5{c}_1-{\mathrm{rc}}_2+R)}^2+{(l_5{s}_1-{\mathrm{rs}}_2)}^2=l_1^2\\ {(l_5{c}_1+{\mathrm{rc}}_2-R)}^2+{(l_5{s}_1+{\mathrm{rs}}_2)}^2=l_3^2\end{array}\right.---\left(3\right)$

$\left\{\begin{array}{c}{l}_5^2+r^2+R^2-l_1^2-2l_5{\mathrm{rc}}_1{c}_2-2l_5{\mathrm{rs}}_1{s}_2+2l_5{\mathrm{Rc}}_1-2r{\mathrm{Rc}}_2=0\\ l_5^2+r^2+R^2-l_3^2+2l_5{\mathrm{rc}}_1{c}_2+2l_5{\mathrm{rs}}_1{s}_2-2l_5{\mathrm{Rc}}_1-2r{\mathrm{Rc}}_2=0\end{array}\right.---\left(4\right)$

(4a)+(4b)，整理可得：

$c_2=({2l}_5^2+2r^2+2R^2-l_1^2-l_3^2)/4\mathrm{rR}---\left(5\right)$

由上式可求解出θ₂的值，值得注意的是θ₂有两个解。将θ₂的值代入式(4a)，可得：

$(-2l_5{\mathrm{rc}}_2+2l_{5}R)c_1+-2l_5{\mathrm{rs}}_2{s}_1+(-2r{\mathrm{Rc}}_2+l_5^2+r^2+R^2-l_1^2)=0---\left(6\right)$

由上式可求解出θ₁的值，θ₁有两个解。

动平台与其相固连的坐标系R’相对于基础坐标系R的姿态可通过如下的两次旋转变操得到：(1)绕Y轴旋转角度θ₂，(2)绕X’轴(由旋转变换1得到)旋转角度θ₃。于是旋转变换矩阵R可表示为：

$R=R(y,{\mathrm{\θ}}_2)R(x^{\′},{\mathrm{\θ}}_3)=\left[\begin{array}{ccc}{c}_2& 0& s_2\\ 0& 1& 0\\ -s_2& 0& c_2\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& c_3& -s_3\\ 0& s_3& c_3\end{array}\right]$

$=\left[\begin{array}{ccc}{c}_2& s_2{s}_3& s_2{c}_3\\ 0& c_3& -s_3\\ -s_2& c_2{s}_3& c_2{c}_3\end{array}\right]$

动平台上铰接点B₂在基础坐标系R中的坐标可表示为：

$B_2=\left[\begin{array}{c}{l}_5{c}_1\\ 0\\ l_5{s}_1\end{array}\right]+\left[\begin{array}{ccc}{c}_2& s_2{s}_3& s_2{c}_3\\ 0& c_3& -s_3\\ -s_2& c_2{s}_3& c_3{c}_3\end{array}\right]\left[\begin{array}{c}0\\ -r\\ 0\end{array}\right]=\left[\begin{array}{c}{l}_5{c}_1-{\mathrm{rs}}_2{s}_3\\ -{\mathrm{rc}}_3\\ l_5{s}_1-{\mathrm{rc}}_2{s}_3\end{array}\right]---\left(7\right)$

驱动腿的长度l₂可以用铰链点A₂、B₂的坐标表示出来，这就是机构的约束方程：

${(l_5{c}_1-{\mathrm{rs}}_2{s}_3)}^2+{(-{\mathrm{rc}}_3+R)}^2+{(l_5{s}_1-{\mathrm{rc}}_2{s}_3)}^2=l_2^2$

(8)

上式整理后可得：

$(-2l_5{\mathrm{rc}}_1{s}_2-2l_5{\mathrm{rs}}_1{c}_2)s_3-2{\mathrm{rRc}}_3+(l_5^2+r^2+R^2-l_2^2)=0---\left(9\right)$

由上式可求解出θ₃的值，θ₃有两个解。

动平台上铰接点B₄在基础坐标系R中的坐标表示为：

$B_4=\left[\begin{array}{c}{l}_5{c}_1\\ 0\\ l_5{s}_1\end{array}\right]+\left[\begin{array}{ccc}{c}_2& s_2{s}_3& s_2{c}_3\\ 0& c_3& -s_3\\ -s_2& c_2{s}_3& c_2{c}_3\end{array}\right]\left[\begin{array}{c}0\\ r\\ 0\end{array}\right]=\left[\begin{array}{c}{l}_5{c}_1+{\mathrm{rs}}_2{s}_3\\ {\mathrm{rc}}_3\\ l_5{s}_1+{\mathrm{rc}}_2{s}_3\end{array}\right]---\left(10\right)$

而：A₄＝(0，R，0)^T

驱动腿的长度l₄可以用铰链点A₄、B₄的坐标表示出来：

$l_4=\sqrt{{(l_5{c}_1+{\mathrm{rs}}_2{s}_3)}^2+{({\mathrm{rc}}_3-R)}^2+{(l_5{s}_1+{\mathrm{rc}}_2{s}_2)}^2}---\left(11\right)$

l₄即为故障传感器当前的应测值，数据重构单元将该应测值送入PID控制单元以代替故障传感器的检测值对驱动腿四的伺服电机进行PID控制，从而实现对驱动腿四传感器故障的容错和自动纠错，确保了并联机器人的正常运行。

Claims (5)

1.一种带容错和纠错功能的并联机器人装置，包括动平台、固定平台及将两平台活动连接以形成并联闭环结构的四条驱动腿，其中两个驱动腿各含有两个单自由度运动副、一个球铰链，另外两个驱动腿各含有一个单自由度运动副和两个球铰链或一个球铰链和一个虎克铰，设于各驱动腿上的含传感器、伺服驱动器、伺服电机在内的传动系统，以及与各传动系统连接以控制机器人运动的控制系统，关键在于在动平台中心与固定平台的中心之间还设有一条柔索，在与固定平台连接点外侧的该柔索上、还串接有用于监测两中心之间距离变化的位移传感器和使柔索呈张紧状态的弹簧，而控制系统则为含现场可编程门阵列逻辑控制器的控制系统；柔索的一端与动平台中心固定，另一端则与固定平台的中心可相对滑动式连接、并在连接点外侧的柔索上串接位移传感器和张紧弹簧后再紧固于固定平台上，该位移传感器还通过信号线与控制器连接、以传送监测信息。

2.按权利要求1所述带容错和纠错功能的并联机器人装置，其特征在于所述含现场可编程门阵列逻辑控制器的控制系统，其控制系统包括闪存器，随机存取存储器及与之连接的含中心控制单元、用于与PC机连接的通讯单元、用于插补计算的插补运算单元、用作PID控制的PID控制单元、用于处理反馈信号的计数单元、用于将反馈参数与输入闪存器中对应的运行参数进行比较的故障检测与隔离单元、将该故障检测值进行隔离处理的缓冲单元、以及用于计算故障传感器应测值的数据重构单元在内的现场可编程门阵列逻辑控制器。

3.按权利要求1所述带容错和纠错功能的并联机器人装置，其特征在于所述柔索为在长度方向的弹性变形量足够小的金属或非金属类柔索；而与固定平台中心的连接则通过环扣或环勾作可相对滑动式连接。

4.按权利要求1所述并联机器人装置的容错和纠错方法，包括：

A.状态监测与故障诊断：首先根据并联机器人的运动要求，通过轨迹规划，将其各条驱动腿长度、速度随时间变化的运行参数输入控制系统的闪存器(FLASH)内，作为驱动腿长度、速度的基础参数，该参数值同时经逻辑控制器中的插补运算单元处理后将结果送入PID控制单元用于对各驱动腿内的伺服电机进行控制；四条驱动腿传感器及柔索传感器的在线检测值一路经缓冲单元输入PID控制单元对伺服电机进行PID控制，另一路在中心控制单元的控制下作为检测值输入随机存取存储器(RAM)、以对并联机器人的运行状况进行在线控制和监测；由四条驱动腿传感器检测值根据空间并联机构的约束关系可以计算出中间柔索传感器的计算值，并将此柔索传感器的计算值与柔索传感器的实际检测值比较，如果两者之间的偏差值超出了设定范围，则表明有一条驱动腿的传感器出现了故障； 

B.故障定位：故障检测与隔离系统将驱动腿传感器此时的基础参数与驱动腿传感器的实际输出值进行对比，如果某一驱动腿两者之间的偏差值超出了设定范围，则判定该驱动腿传感器为故障传感器；

C.故障传感器的容错和纠错：当由B步骤确定出故障传感器后，即由另外三条驱动腿传感器的检测值和中间柔索传感器的检测值通过FPGA运动控制器中的数据重构单元，计算出故障传感器的应测值，并在检测与隔离单元的控制下通过缓冲器将故障检测值丢弃，以该应测值替换，用于对故障传感器所对应的伺服电机进行控制，从而实现对故障传感器的容错和纠错。

5.按权利要求4所述并联机器人装置的容错和纠错方法，其特征在于所述故障传感器的故障包括驱动腿传感器本身故障或其电路传输故障、或传感器探头因物理障碍所致异常输出。 

Images