基于主从空间映射的主从异构机器人通用控制方法及系统与流程

本发明属于遥操作机器人领域，涉及一种主从异构机器人的通用主从空间映射与控制方法。

背景技术：

随着各类技术的不断发展，机器人已经逐渐进入到生产和生活的各个领域，比如在许多工业现场的生产线上只需要调好程序，机器人就可以自动的完成生产任务。然而，在许多的工业控制现场、化工厂、核工厂等等，由于工厂的环境复杂，并且危险性较大，考虑到工人的安全就不能采用人工的方式，而且由于现场的状况非常复杂很难实现生产过程的完全自动化，两者进行综合就产生了主从式遥操作型机器人。另外，在医学领域的微创手术机器人也是采用的遥操作机器人方式，通过这种主从式的控制方式可以使手术的操作更加的精确、效果更好。

遥操作机器人主要采用主从式的操作方法，包括主手端机器人以及从手端机器人两个部分，根本思想是通过检测主手的各个关节的运动角度，来实现对从手端机器人的控制。由于从手端机器人的运动对于整个生产过程的安全性以及精确性都具有很大的影响，所以从手端的机器人需要准确的检测到主手端的控制信号，并且根据主手端的控制信号进行相应的动作。主手端的机器人的位置以及速度信号主要是通过各个关节的编码器来获得其相应转动的角度信息，然后传送到控制器中，最后在经过相应的计算，通过控制器发送相应的信号到从手端机器人，从而控制从手端机器人的运动，完成工业生产或者微创手术等操作。

对于主从手机器人的控制方式，主要存在主从同构型控制方式与主从异构型控制方式两种。由于主从同构型的控制方式要求主手端机器人与从手端机器人的结构形式相同，所以主从同构式操作机器人的适用范围较小；目前存在较多的都是主从手端含有不同结构的机器人，所以一般应采用主从异构型控制方式来实现主手端对从手端的控制。据调查，目前存在的主从异构式控制方法通常只适用于某一种主从类型的机器人，当采用其他类型的主手端机器人或者从手端机器人时，机器人就不能很好的工作，不能达到机器人预期的效果。

技术实现要素：

本发明为了克服现有技术的不足，提供一种适用于任意主从异构型遥操作机器人的基于主从空间映射的主从异构机器人通用控制方法及系统，该方法可以对任意自由度的主手端机器人以及从手端机器人进行空间位置、速度以及力的映射计算，通过主手端机器人位置和速度的识别来实现对从手端机器人位置和速度的精确控制以及实现从手端机器人对主手端机器人的力反馈。

为达到上述目的，本发明采用了以下技术方案：

一种基于主从空间映射的主从异构机器人通用控制方法，该控制方法包括以下步骤：

1)根据主、从手端机器人参数，建立任意自由度的主手端机器人与任意自由度的从手端机器人之间的主从空间映射；

2)利用主从空间映射进行主从空间转换

将采集的主手端机器人的位置信号转化为从手端机器人运动控制信号，将运动控制信号发送到从手端机器人控制器，使从手端机器人进行运动，并且根据反馈的从手端机器人的位置信号进行闭环控制，直至从手端机器人运动达到目标位姿；运动过程中根据目标位姿控制精度要求对从手端机器人进行切换。

所述主从空间转换存在以下两种情况：

①当主手端机器人的自由度大于从手端机器人的自由度时，在满足空间位置的前提下尽量使从手端机器人的姿态接近主手端机器人的姿态；

②当主手端机器人的自由度小于等于从手端机器人的自由度时，从主手端机器人要求达到的空间位置和姿态(即目标位姿)所对应的多个逆解中选择与从手端机器人当前各个关节角度相差最小的逆解来控制从手端机器人的运动。

所述控制方法还包括以下步骤：利用主从空间映射将从手端机器人的力信号通过计算进行转化，并根据转化结果对主手端机器人进行力反馈。

所述控制方法具体包括以下步骤：

①主、从端机器人相关参数的输入

将主、从端机器人的各个连杆的长度和扭转角度、相邻连杆间的关节偏距以及各个关节的运动角度范围输入到控制系统之中；

②主从端运动比例系数的选择

对默认主从端运动比例系数进行更改，默认主从端运动比例系数是根据步骤①输入的相关参数计算主手端机器人工作空间以及从手端机器人工作空间的大小，然后将主手端机器人工作空间与从手端机器人工作空间相比而得到的；

③是否需要选用力反馈的功能选择

经过步骤②后，确定从手端机器人在运动中是否对主手端机器人进行力反馈；

④从手端机器人的安全性检测

若检测到从手端机器人任意关节的转动将要到达奇异位置之后，就直接停止从手端机器人的运动；否则(未到达奇异位置)，转至步骤⑤；

⑤主从空间位置转换

根据主手端机器人各个关节转动角度的信息将主手端机器人的关节空间坐标转化为笛卡尔坐标，并且根据主从端运动比例系数进行放大或缩小，然后根据从手端机器人的相关参数求逆解，将笛卡尔坐标值转化为从手端机器人关节空间坐标值，从而控制从手端机器人的运动；

若在步骤③中确定了需要进行力反馈，则根据从手端机器人各个关节运动的速度变化量与时间的关系来求解各个关节力矩的大小，然后经过从手端机器人的雅克比矩阵转化到笛卡尔空间，再通过主手端机器人的逆雅克比矩阵转化为主手端机器人各个关节力矩的大小，或者，将从手端机器人上的力传感器的测量数据直接经过主手端机器人的逆雅克比矩阵转化为主手端机器人各个关节力矩的大小，将该力矩作用到主手端机器人各个关节以实现力反馈；

⑥在从手端机器人运动的过程中随时监测是否需要停止，当需要停止时设置从手端机器人停止工作，若不需要，则返回步骤④。

若需要对从手端机器人进行切换，则使从手端机器人首先停止，在选择好切换的模式之后再重新启动从手端机器人；切换的模式包括①切换主手端机器人当前控制的从手端机器人及②对主手端机器人的各个关节位置进行回零操作和/或更换主从端运动比例系数。

一种主从异构机器人的通用主从空间映射与控制系统，该控制系统包括主从端机器人结构参数输入模块、主从运动比例系数选择模块、主从端机器人切换控制模块以及主从空间位置转换模块；

所述主从端机器人结构参数输入模块用于将主手端机器人以及对应需要控制的各个从手端机器人的各个连杆的长度和扭转角度、相邻连杆间的关节偏距以及各个关节的运动角度范围输入到控制系统中；

所述主从运动比例系数选择模块用于计算默认主从端运动比例系数以及对默认主从端运动比例系数进行更改，默认主从端运动比例系数是根据主从端机器人结构参数输入模块输入的相关参数计算主手端机器人工作空间以及从手端机器人工作空间的大小，然后将主手端机器人工作空间与从手端机器人工作空间相比而得到的；

所述主从端机器人切换控制模块用于将主手端机器人当前控制的从手端机器人固定到一定的位姿后进行切换模式选择，然后重新启动该主手端机器人控制下的从手端机器人运动；切换模式包括：①切换主手端机器人当前控制的从手端机器人及②对主手端机器人的各个关节位置进行回零操作和/或更换主从端运动比例系数；

所述主从空间位置转换模块用于根据主手端机器人各个关节转动角度的信息将主手端机器人的关节空间坐标转化为笛卡尔坐标，并且根据主从端运动比例系数进行放大或缩小，然后根据从手端机器人的相关参数求逆解，将笛卡尔坐标值转化为从手端机器人关节空间坐标值，从而控制从手端机器人的运动。

所述控制系统还包括主从端机器人安全检测模块、力反馈选择与计算模块以及从手端机器人固定模块。

所述主从端机器人安全检测模块用于实时的对从手端机器人的任意关节的转动位置进行估计，在主手端机器人控制从手端机器人将要运动到关节的奇异位置时，将从手端机器人的运动设置为停止。

所述力反馈选择与计算模块用于根据从手端机器人各个关节运动的速度变化与时间的关系求解各个关节力矩的大小，然后经过从手端机器人的雅克比矩阵转化到笛卡尔空间，再通过主手端机器人的逆雅克比矩阵转化为从手端机器人各个关节力矩的大小；或者，用于将从手端机器人上的力传感器的测量数据直接经过主手端机器人的逆雅克比矩阵转化为主手端机器人各个关节力矩的大小，从而实现力反馈。

所述从手端机器人固定模块用于对任意一个主手端机器人控制的从手端机器人的位姿进行固定。

与现有技术相比，本发明的有益效果是：

本发明利用主从端机器人参数对主手端机器人以及从手端机器人进行建模与空间转化计算，从而准确的将主手端机器人位置信号转化为从手端机器人的运动控制信号，同时，通过对主手端机器人控制下的从手端机器人的切换实现对从手端机器人的精确控制。本发明可以实现主手端机器人对从手端机器人的精确位置和速度(位置比时间)控制，具有很强的通用性以及安全性，适用于任意主手端机器人对任意从手端机器人的控制。

进一步的，本发明可以对各类从手端机器人实现对主手端机器人的力反馈，尤其是当从手端机器人未安装有力传感器时，可以通过对速度的变化与时间的关系估计出各关节力大小，从而转化为主手端机器人的关节力矩，实现从手端机器人对主手端机器人的精确力反馈。

进一步的，本发明可以实时对系统进行安全监测，避免从手端机器人运动到奇异位置而发生事故，可以使得主从控制更加的安全。

附图说明

图1为主从控制原理图。

图2为通用主从空间映射与控制系统构成图。

图3为通用主从空间映射与控制系统流程图。

具体实施方式

下面结合附图和实施例对本发明做进一步描述。应当理解，此处描述的实施例仅仅用以解释本发明，并不限制本发明。

如图1所示，本发明将任意自由度的主手端机器人与任意自由度的从手端机器人通过中间的主从空间映射与控制系统来联系起来，主从空间映射与控制系统的功能主要是将主手端机器人的位置信号收集并且进行转化，然后将转化得到的运动控制信号发送到从手端机器人控制器进行动作，并且将从手端机器人的位置和速度信号反馈到主从空间映射与控制系统，由主从空间映射与控制系统进行闭环控制达到目标位姿。同时，主从空间映射与控制系统还可以将从手端机器人的力信号通过计算转化到主手端进行力的反馈。

为实现以上部分的功能，如图2所示，所述主从空间映射与控制系统包括：主从端机器人结构参数输入模块、主从运动比例系数选择模块、主从端机器人切换控制模块、主从空间位置转换模块、主从端机器人安全检测模块、力反馈选择与计算模块及从手端机器人固定模块。控制系统根据输入参数采用d-h参数法和雅克比矩阵法对主从手的位置、速度以及力进行映射与转换，从而实现主手对从手的精确控制。各个模块的主要功能如下：

所述主从端机器人结构参数输入模块：由于本发明设计的是通用主从异构映射与控制，所以主手端与从手端的机器人的参数不是固定的，所以在进行控制之前需要对控制系统的参数进行配置。由于系统对于机器人的描述采用的是d-h参数法，这时就需要通过主从端机器人结构输入模块将主手端与从手端机器人的连杆数量、各个连杆的长度和扭角以及相邻连杆间的关节偏距、各个关节的运动角度的大小输入到控制系统中，完成对系统的配置以便于后面对于系统主从空间映射参数的计算。

所述主从运动比例系数选择模块：由于在通过主手端机器人对从手端机器人进行控制的时候，主手端机器人的工作空间以及从手端机器人的工作空间的大小都是固定的(蒙特卡洛法计算)，而要实现较好的控制效果，一方面需要从手端机器人能够在主手端机器人的控制下达到从手端机器人工作空间内的任意位置和姿态，另一方面，从手端机器人在进行精细操作的时候必须要使从手端机器人运动的精度更高、速度更慢，而主手端机器人一般是通过人手来操作，所以对于精度的控制不好把握。这时主从运动比例系数选择模块就可以起到非常大的作用：在进行操作之前该模块可以根据前面主从端机器人结构参数输入模块的参数输入对主手端机器人的工作空间与从手端机器人的工作空间的比例系数进行计算，默认采用比例系数为主手端机器人的工作空间大小与从手端机器人的工作空间大小之比(例如1:2)，来使从手端机器人遍历其整个工作空间，当需要进行精细操作时，就可以调整比例系数(例如5:1)，使得主手端机器人控制的从手端机器人的运动精度更高(控制精度要求越高，则比例系数越大)。

所述主从端机器人切换控制模块：在进行从手端机器人运动控制时候，有时需要三个从手端机器人工作才能完成，然而主手端机器人的控制最多通过左右两手来操作两个主手端机器人对从手端的两个机器人进行控制，所以就需要采用主从端机器人切换控制模块将其中一个主手端机器人控制的从手端机器人固定到一定的位姿，然后将这个主手端机器人切换到另外的从手端机器人上面进行控制。另外，由于前面运动比例系数的选择，主手端机器人可能不能控制从手端机器人运动到任意位置，当主手端机器人达到极限位置而从手端机器人还未达到目标位姿的时候，就可以采用主从端机器人切换控制模块固定从手端机器人的位置不动，并将主手端机器人的各个关节位置进行回零操作，然后重启主手端机器人对该从手端机器人的控制。

所述主从空间位置转换模块：由于控制系统需要实现任意主手端机器人可以控制任意从手端机器人的运动，并且主、从手端机器人的连杆各参数已经输入到系统中，这时就需要根据主手端机器人输入的各个关节转动角度的信息经过机器人运动学正解的计算(d-h参数法)将主手端机器人的关节空间坐标转化为笛卡尔坐标，然后根据运动比例系数进行放大或者缩小得到一个笛卡尔空间的坐标值，根据得到的该坐标值对从手端机器人进行逆解的计算，即将得到的笛卡尔空间坐标系的坐标值转化为从手端机器人的关节空间坐标值，实现对从手端机器人的运动控制。另外此模块还可以对从手端机器人的运动形成闭环，来精确的达到从手端机器人的目标位姿。由于本发明是一种通用的主从空间映射方法，所以在进行主从空间转换时，主要存在以下两种情况：

①当主手端机器人的自由度大于从手端机器人的自由度时，由上面所述，在将主手端机器人的关节空间坐标转化为笛卡尔空间坐标并经过运动比例系数缩放之后，需要对从手端机器人进行逆解计算，由于从手端机器人的自由度较少，对于主手端机器人的一些姿态无法达到，这时系统会在满足空间位置的前提下尽量使从手端机器人的姿态接近主手端机器人的姿态；

②当主手端机器人的自由度小于等于从手端机器人的自由度时，由于从手端机器人的自由度较多，所以对于主手端机器人要求达到的空间位置和姿态存在较多的逆解，这时系统可以对逆解进行评估，选择与从手端机器人当前各个关节角度相差较小的逆解来控制从手端机器人的运动。

所述主从端机器人安全检测模块：在对从手端机器人进行控制的时候，从手端机器人总是会存在许多的奇异位置，当从手端机器人关节转动达到奇异位置的时候，会发生各种各样的危险事故，所以在系统中需要实时的对从手端机器人的运动安全性进行检测，在主手端机器人控制从手端机器人将要运动到其奇异位置的时候，为了避免事故发生，将从手端机器人的运动设置为停止。

所述力反馈选择与计算模块：在主手端机器人控制从手端机器人进行运动的时候，为了达到更好的运动效果，一般的，主手端机器人都会设计含有力反馈的功能，本发明可以适用于含有以及未含有力反馈的任意主手端机器人。在进行主从控制之前，操作者需要在系统中输入是否需要力反馈的功能，由于一般从手端机器人安装力传感器的较少而主手端机器人一般都具有力反馈的功能，所以本模块含有力估计器，可以根据从手端机器人各个关节运动的速度变化与时间的关系计算出从手端机器人关节力矩的大小，然后经过从手端机器人的雅克比矩阵转化到笛卡尔空间，最后再通过主手端机器人的逆雅克比矩阵转化为主手端机器人各个关节力矩的大小。如果从手端机器人的末端安装有力传感器，便可以直接经过主手端机器人的逆雅克比矩阵转化为主手端机器人各个关节力矩的大小，直接实现力反馈。

所述从手端机器人固定模块：在进行从手端机器人的控制操作时，有时为了完成一道工序，需要从手端机器人固定到某个位姿，然后控制另外一个主手端机器人进行运动，如果采用固定人的手不动从而主手端机器人不动的方式来控制从手端机器人固定，效果很难达到工艺的要求，这时就可以通过从手端机器人固定模块来实现对从手端机器人的固定，当工序完成之后，便可以重新启动从手端机器人继续完成后面的工作。

应用实例

选用两个六自由度机器人作为主手端机器人并且在主手端机器人的每个关节均安装有力矩电机，可以实现力反馈，从手端机器人选用的是三个六自由度机器人，并且从手端机器人的各个关节以及末端并没安装任何力传感器。将主手端机器人与主从空间映射与控制系统连接，然后再将该控制系统与从手端机器人进行连接，从而实现主手端机器人对从手端机器人的精确控制。

如图3所示，具体的控制流程为：

初始化：在主、从手端机器人分别与控制系统连接成功以后，首先进行主从端六自由度机器人相关参数的输入，将主、从端机器人的各个连杆的长度、扭转角度、相邻关节的偏距以及各个关节的运动角度范围输入到控制系统之中；然后进行主从端运动比例系数的选择，一般系统默认根据前面输入的参数计算主手端机器人工作空间以及从手端机器人工作空间的大小，然后将主手端机器人工作空间与从手端机器人工作空间相比，就得到了主从端的默认运动比例系数，并将系数显示给操作者，操作者根据需要可以对默认运动比例系数进行更改；在设置完主从运动比例系数之后，需要选择是否需要选用力反馈的功能，由于选用的主手端六自由度机器人具有六个自由度的力反馈，所以这里选择需要力反馈。

在对整个主从空间映射与控制系统进行初始化之后，需要对从手端六自由度机器人的安全性进行检测，在检测到从手端机器人将到达奇异位置之后，就直接停止从手端机器人的运动，只有当从手端机器人没有到达奇异位置，才能进行下一步的主从空间力位映射控制(主手端机器人与从手端机器人工作空间位置转化和力反馈)。在检测安全之后，控制系统将根据主手端机器人输入的各个关节转动角度的信息将主手端机器人的关节空间坐标转化为笛卡尔坐标，并且根据主从端运动比例系数进行放大或缩小，然后根据从手端六自由度机器人的各个参数求逆解，将笛卡尔坐标值转化为关节空间坐标值，从而控制从手端机器人的运动。由于从手端的六自由度机器人不具有力传感器，但是仍需要进行力反馈，这时就需要通过控制系统的力估计器，根据从手端机器人的速度变化量与时间的关系来求解各个关节力矩的大小，然后经过从手端机器人的雅克比矩阵转化到笛卡尔空间，再通过主手端机器人的逆雅克比矩阵转化为主手端机器人各个关节的力矩，并作用到主手端机器人各个关节以实现力反馈。

在运动的过程中，如果需要将从手端某个六自由度机器人设置为某一固定位姿，就可以将从手端机器人选择为固定模式，以便于从手端机器人可以更好的工作。另外，在从手端机器人运动的过程中，可以对从手端机器人进行切换，确认切换之后需要选择是切换主从端运动比例系数(主手端机器人回零)以便更加精确的工作，还是切换主手端机器人去控制另外一个从手端的六自由度机器人。当执行切换时，从手端机器人首先停止，在选择好切换的模式之后再重新启动从手端机器人运动。在运动的过程中随时监测是否需要停止，当主手端机器人的操作者需要停止操作时，就可以设置从手端机器人停止工作。

总之，本发明提出了一种适用于任何主从异构型遥操作机器人的主从空间映射控制方法，其可以对任意自由度的主手端机器人以及从手端机器人进行空间位置、速度以及力进行映射计算，通过主手端机器人位置、速度的识别来实现对从手端机器人位置和速度的精确控制以及实现从手对主手的力反馈。