CN114074329A

CN114074329A - 一种机器人远心不动点的校验控制方法及相关装置

Info

Publication number: CN114074329A
Application number: CN202210057244.8A
Authority: CN
Inventors: 李耀; 其他发明人请求不公开姓名
Original assignee: Chengdu Borns Medical Robotics Co Ltd
Current assignee: Chengdu Borns Medical Robotics Co Ltd
Priority date: 2022-01-19
Filing date: 2022-01-19
Publication date: 2022-02-22
Anticipated expiration: 2042-01-19
Also published as: CN114074329B

Abstract

本申请公开了一种机器人远心不动点的校验控制方法及相关装置，所述方法包括：离线采集机器人的机械臂的RCM点的起始位置，并利用光学传感器采集床体的传感标记起始位置；利用转换关系、RCM点的起始位置和传感标记起始位置得到RCM点和传感标记的初始相对位置；在线采集RCM点的当前位置，根据RCM点的当前位置和RCM点的起始位置计算RCM点的相对位置变化量；根据RCM点的相对位置变化量确定传感标记的期望位置，根据期望位置控制床体运动。本申请能够提高机械臂RCM点和传感标记的坐标匹配精度，实现机械臂与床体的高精度跟随。

Description

一种机器人远心不动点的校验控制方法及相关装置

技术领域

本申请涉及机器人控制技术领域，特别涉及一种机器人远心不动点的校验控制方法及相关装置。

背景技术

在机器人操作过程中，需要将床体和机械臂进行操作前匹配，以使实现二者坐标系一致。本领域中通常获取床体的偏移量，计算获得机械臂各个关节数据，完成机械臂跟随床体运动，保证跟随的位置和角度精确。由于机械控制误差、信号传递误差或数据误差等因素，存在机械臂RCM（Remote Center of Motion，远程运动中心）点与床传感标记之间的相对位置发生偏移的情况。

因此，如何提高机械臂RCM点和传感标记的坐标匹配精度，实现机械臂与床体的高精度跟随是本领域技术人员目前需要解决的技术问题。

发明内容

本申请的目的是提供一种机器人远心不动点的校验控制方法及相关装置，能够提高机械臂RCM点和传感标记的坐标匹配精度，实现机械臂与床体的高精度跟随。

为解决上述技术问题，本申请提供一种机器人远心不动点的校验控制方法，该机器人远心不动点的校验控制方法包括：

离线采集机器人的机械臂的RCM点的起始位置，并利用光学传感器采集床体的传感标记起始位置；其中，所述RCM点的起始位置为所述机械臂的RCM点在机械臂基坐标系中的位置，所述传感标记起始位置为所述床体上的传感标记在光学传感器坐标系中的初始位置；

确定所述机械臂基坐标系和所述光学传感器坐标系的转换关系，并利用所述转换关系、所述RCM点的起始位置和所述传感标记起始位置得到所述RCM点和所述传感标记的初始相对位置；

在所述机械臂执行摆位操作时在线采集所述RCM点的当前位置，并根据所述RCM点的当前位置和所述RCM点的起始位置计算所述RCM点的相对位置变化量；

根据所述RCM点的相对位置变化量确定所述传感标记的期望位置，并根据所述期望位置控制所述床体运动，以使所述传感标记运动至所述期望位置；其中，所述RCM点的当前位置和所述期望位置的相对位置为所述初始相对位置。

可选的，在根据所述期望位置控制所述床体运动之后，还包括：

判断所述床体或所述传感标记的位置是否发生变化；

若是，则控制所述机械臂跟随所述床体或所述传感标记围绕所述RCM点运动。

可选的，判断所述床体或所述传感标记的位置是否发生变化，包括：

利用所述光学传感器采集所述传感标记的当前位置；

根据所述传感标记的当前位置计算所述传感标记在所述光学传感器坐标系中的相对位置变化量；

判断所述传感标记在所述光学传感器坐标系中的相对位置变化量是否为零；

若是，则判定所述床体或所述传感标记的位置未发生变化；

若否，则判定所述床体或所述传感标记的位置发生变化。

可选的，控制所述机械臂跟随所述床体或所述传感标记围绕所述RCM点运动，包括：

根据所述传感标记在所述光学传感器坐标系中的相对位置变化量，计算所述RCM点的第一目标位置；其中，所述传感标记的当前位置和所述第一目标位置之间的相对位置为所述初始相对位置；

控制所述机械臂执行跟随床体运动，以使所述RCM点运动至所述第一目标位置。

可选的，根据所述传感标记在光学传感器坐标系中的相对位置变化量，计算所述RCM点的第一目标位置，包括：

根据所述转换关系对所述传感标记在光学传感器坐标系中的相对位置变化量进行坐标转换，得到所述传感标记的在所述机械臂基坐标系中的相对位置变化量；

根据所述传感标记的在所述机械臂基坐标系中的相对位置变化量计算所述RCM点在所述机械臂基坐标系中的第一目标位置；

相应的，控制所述机械臂执行跟随床体运动包括：

根据所述RCM点在所述机械臂基坐标系中的第一目标位置控制所述机械臂执行所述跟随床体运动。

可选的，在所述传感标记运动至所述期望位置之后，还包括：

判断所述RCM点的位置是否发生变化；

若是，则控制所述床体跟随所述RCM点运动。

可选的，判断所述RCM点的位置是否发生变化，包括：

获取所述RCM点的当前位置，根据所述RCM点的当前位置计算所述RCM点在所述机械臂基坐标系中的相对位置变化量；

判断所述RCM点在所述机械臂基坐标系中的相对位置变化量是否为零；

若是，则判定所述RCM点的位置未发生变化；

若否，则判定所述RCM点的位置发生变化。

可选的，控制所述床体跟随所述RCM点运动，包括：

根据所述RCM点在机械臂基坐标系中的相对位置变化量，计算所述传感标记的第二目标位置；其中，所述RCM点的当前位置和所述第二目标位置之间的相对位置为所述初始相对位置；

控制所述床体跟随机械臂运动，以使得所述传感标记运动至所述第二目标位置。

可选的，根据所述RCM点在机械臂基坐标系中的相对位置变化量，计算所述传感标记的第二目标位置，包括：

根据所述转换关系对所述RCM点在机械臂基坐标系中的相对位置变化量进行坐标转换，得到所述RCM点的在所述机械臂基坐标系中的相对位置变化量；

根据所述RCM点的在所述机械臂基坐标系中的相对位置变化量计算所述传感标记在所述机械臂基坐标系中的第二目标位置；

相应的，控制所述床体执行跟随机械臂运动包括：

根据所述传感标记在所述机械臂基坐标系中的第二目标位置控制所述床体执行所述跟随机械臂运动。

可选的，在所述机械臂执行摆位操作时在线采集所述RCM点的当前位置，包括：

在所述机械臂执行摆位操作时在线采集所述机械臂的关节参数变化量；

对所述关节参数变化量进行正运动学计算，得到所述RCM点的当前位置。

可选的，在所述机械臂执行摆位操作时在线采集所述RCM点的当前位置，并根据所述RCM点的当前位置和所述RCM点的起始位置计算所述RCM点的相对位置变化量，包括：

在所述机械臂执行摆位操作时在线采集所述RCM点在所述机械臂基坐标系中的当前位置，并根据所述RCM点在所述机械臂基坐标系中的RCM点的起始位置计算所述RCM点在所述机械臂基坐标系中的相对位置变化量；

根据所述转换关系对所述RCM点在所述机械臂基坐标系中的相对位置变化量进行坐标转换，得到所述RCM点在所述光学传感器坐标系中的相对位置变化量。

可选的，利用所述转换关系、所述RCM点的起始位置和所述传感标记起始位置得到所述RCM点和所述传感标记的初始相对位置，包括：

利用所述转换关系、所述RCM点的起始位置和所述传感标记起始位置得到所述RCM点和所述传感标记在所述机械臂基坐标系中的初始相对位置；

相应的，根据所述RCM点的相对位置变化量确定所述传感标记的期望位置，包括：

根据所述RCM点在所述机械臂基坐标系中的相对位置变化量确定所述传感标记在所述机械臂基坐标系中的期望位置。

利用所述转换关系、所述RCM点的起始位置和所述传感标记起始位置得到所述RCM点和所述传感标记在所述光学传感器坐标系中的初始相对位置；

根据所述RCM点在所述光学传感器坐标系中的相对位置变化量确定所述传感标记在所述光学传感器坐标系中的期望位置。

本申请还提供了一种机器人远心不动点的校验控制的相关装置，该相关装置包括存储介质和机器人。

本申请还提供了一种存储介质，其上存储有计算机程序，所述计算机程序执行时实现上述机器人远心不动点的校验控制方法执行的步骤。

本申请还提供了一种机器人，包括机械臂、底座、存储器和处理器，所述底座设置有光学传感器，所述存储器中存储有计算机程序，所述处理器调用所述存储器中的计算机程序时实现上述机器人远心不动点的校验控制方法执行的步骤。

本申请提供了一种机器人远心不动点的校验控制方法，包括：离线采集机器人的机械臂的RCM点的起始位置，并利用光学传感器采集床体的传感标记起始位置；其中，所述RCM点的起始位置为所述机械臂的RCM点在机械臂基坐标系中的位置，所述传感标记起始位置为所述床体上的传感标记在光学传感器坐标系中的初始位置；确定所述机械臂基坐标系和所述光学传感器坐标系的转换关系，并利用所述转换关系、所述RCM点的起始位置和所述传感标记起始位置得到所述RCM点和所述传感标记的初始相对位置；在所述机械臂执行摆位操作时在线采集所述RCM点的当前位置，并根据所述RCM点的当前位置和所述RCM点的起始位置计算所述RCM点的相对位置变化量；根据所述RCM点的相对位置变化量确定所述传感标记的期望位置，并根据所述期望位置控制所述床体运动，以使所述传感标记运动至所述期望位置；其中，所述RCM点的当前位置和所述期望位置的相对位置为所述初始相对位置。

本申请离线采集机械臂的RCM点在机械臂基坐标系中的位置，还利用光学传感器采集床体上的传感标记在光学传感器坐标系中的初始位置。RCM点的起始位置所在的坐标系为机械臂基坐标系，传感标记起始位置所在的坐标系为光学传感器坐标系，本申请确定机械臂基坐标系和光学传感器坐标系的转换关系，进而利用上述转换关系可以将机械臂基坐标系和光学传感器坐标系进行转换得到RCM点和所述传感标记的初始相对位置。本申请在线采集RCM点的当前位置和RCM点的起始位置确定RCM点的相对位置变化量，基于RCM点的相对位置变化量传感标记的期望位置，并利用期望位置指导床体运动，以使所述RCM点的当前位置和所述传感标记的当前位置的相对位置为所述初始相对位置。上述过程能够提高机械臂RCM点和传感标记的坐标匹配精度，实现机械臂与床体的高精度跟随。本申请同时还提供了一种机器人远心不动点的校验控制方法的相关装置，具有上述有益效果，在此不再赘述。

附图说明

为了更清楚地说明本申请实施例，下面将对实施例中所需要使用的附图做简单的介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本申请实施例所提供的一种机器人远心不动点的校验控制方法的流程图。

具体实施方式

为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

下面请参见图1，图1为本申请实施例所提供的一种机器人远心不动点的校验控制方法的流程图。

具体步骤可以包括：

S101：离线采集机器人的机械臂的RCM点的起始位置，并利用光学传感器采集床体的传感标记起始位置；

其中，本实施例可以应用于手术机器人、喷漆机器人、零件装配机器人等机器人，该机器人包括机械臂和底座，机械臂的末端安装有操作器械，操作器械可以对设置于床体上的被操作对象进行操作。在机械臂工作过程中，围绕RCM点运动。

在机器人未处于工作状态时，本实施例可以通过离线测量的方式（如通过尺子测量）采集机器人的机械臂的RCM点的起始位置，上述RCM点的起始位置为所述机械臂的RCM点在机械臂基坐标系中的位置。

机械臂的操作对象设置于床体，床体上可以设置有传感标记，即光学标记。机器人的基座上可以设置有光学传感器，用于检测床体上传感标记起始位置。上述传感标记起始位置为所述床体上的传感标记在光学传感器坐标系中的初始位置。

本实施例中RCM点的起始位置与传感标记起始位置为机器人在同一时刻下测量的两个参数。

S102：确定机械臂基坐标系和光学传感器坐标系的转换关系，并利用转换关系、RCM点的起始位置和传感标记起始位置得到RCM点和传感标记的初始相对位置；

其中，机械臂基坐标的原点可以设置于机械臂与基座或伸缩臂连接位置的中心点，光学传感器坐标系为以光学传感器所在位置为原点的坐标系。本实施例可以预先确定机械臂基坐标系和光学传感器坐标系的转换关系，基于该转换关系对RCM点的起始位置或传感标记起始位置进行坐标系转换，进而得到RCM点和传感标记的初始相对位置。

作为一种可行的实施方式，可以利用转换关系将RCM点的起始位置转换至光学传感器坐标系，进而结合传感标记起始位置得到RCM点和传感标记在光学传感器坐标系下的初始相对位置。

作为另一种可行的实施方式，可以利用转换关系将传感标记起始位置转换至机械臂基坐标系，进而结合RCM点的起始位置得到RCM点和传感标记在光学传感器坐标系下的初始相对位置。

S103：在所述机械臂执行摆位操作时在线采集所述RCM点的当前位置，并根据所述RCM点的当前位置和所述RCM点的起始位置计算所述RCM点的相对位置变化量；

其中，在机械臂进入工作状态之前，可以存在摆位操作。为了避免摆位操作对于RCM点和传感标记的坐标匹配精度，本申请在线采集RCM点的当前位置，进而根据所述RCM点的当前位置和所述RCM点的起始位置计算所述RCM点的相对位置变化量，以便根据RCM点的相对位置变化量对床体的位置进行调整。

S104：根据所述RCM点的相对位置变化量确定所述传感标记的期望位置，并根据所述期望位置控制所述床体运动，以使所述传感标记运动至所述期望位置；

其中，在得到RCM点的相对位置变化量的基础上，本实施例可以将初始相对位置作为跟随运动的匹配基准，进而根据RCM点的相对位置变化量确定所述传感标记的期望位置，RCM点的当前位置和期望位置的相对位置为所述初始相对位置。在得到期望位置的基础上，本申请可以所述期望位置控制所述床体运动，以使所述传感标记运动至所述期望位置，进而实现机械臂和床体的自动跟随。

本实施例离线采集机械臂的RCM点在机械臂基坐标系中的位置，还利用光学传感器采集床体上的传感标记在光学传感器坐标系中的初始位置。RCM点的起始位置所在的坐标系为机械臂基坐标系，传感标记起始位置所在的坐标系为光学传感器坐标系，本实施例确定机械臂基坐标系和光学传感器坐标系的转换关系，进而利用上述转换关系可以将机械臂基坐标系和光学传感器坐标系进行转换得到RCM点和所述传感标记的初始相对位置。本实施例在线采集RCM点的当前位置和RCM点的起始位置确定RCM点的相对位置变化量，基于RCM点的相对位置变化量传感标记的期望位置，并利用期望位置指导床体运动，以使所述RCM点的当前位置和所述传感标记的当前位置的相对位置为所述初始相对位置。上述过程能够提高机械臂RCM点和传感标记的坐标匹配精度，实现机械臂与床体的高精度跟随。本实施例通过机械臂RCM点的效验，完成与床的坐标匹配，提高机械臂与床位置及姿态的跟随精度。

作为对于图1对应实施例的进一步介绍，在根据所述期望位置控制所述床体运动之后，还可以存在机械臂跟随床体运动的操作，具体过程如下：若所述机械臂执行摆位操作完毕，则获取所述传感标记的当前位置；根据所述传感标记的当前位置和所述期望位置计算所述传感标记的相对位置变化量，并根据所述传感标记的相对位置变化量计算所述RCM点的目标位置；其中，所述传感标记的当前位置和所述目标位置之间的相对位置为所述初始相对位置。控制所述机械臂执行床体跟随运动，以使所述RCM点运动至所述目标位置。

本实施例可以根据所述传感标记的当前位置和所述期望位置计算所述传感标记在所述光学传感器坐标系中的相对位置变化量；根据所述转换关系生成所述传感标记的在所述机械臂基坐标系中的相对位置变化量；根据所述传感标记的在所述机械臂基坐标系中的相对位置变化量计算所述RCM点在所述机械臂基坐标系中的目标位置。RCM点的当前位置与传感标记的当前位置在机械臂基坐标系中的相对位置为初始相对位置（机械臂基坐标系）。

本实施例还可以根据所述传感标记的当前位置和所述期望位置计算所述传感标记在所述光学传感器坐标系中的相对位置变化量；根据所述转换关系生成RCM点在光学传感器坐标系中的当前位置，根据传感标记在所述光学传感器坐标系中的相对位置变化量和RCM点在光学传感器坐标系中的当前位置计算RCM点在光学传感器坐标系中的目标位置。RCM点的当前位置与传感标记的当前位置在光学传感器坐标系中的相对位置为初始相对位置（光学传感器坐标系）。

作为一种可行的实施方式，图1对应的实施例可以通过以下方式在线采集RCM点的当前位置：在所述机械臂执行摆位操作时在线采集所述机械臂的关节参数变化量；对所述关节参数变化量进行正运动学计算，得到所述RCM点的当前位置。

作为一种可行的实施方式，机械臂可以设置于伸缩臂的一端，随着伸缩臂在水平方向或竖直方向的变化机械臂基坐标系的原点会发生变化，因此图1对应的实施例可以通过以下方式计算RCM点的相对位置变化量包括：在所述机械臂执行摆位操作时在线采集所述RCM点在所述机械臂基坐标系中的当前位置，并根据所述RCM点在所述机械臂基坐标系中的RCM点的起始位置计算所述RCM点在所述机械臂基坐标系中的相对位置变化量；根据所述转换关系对所述RCM点在所述机械臂基坐标系中的相对位置变化量进行坐标转换，得到所述RCM点在所述光学传感器坐标系中的相对位置变化量。

作为一种可行的实施方式，本实施例可以通过以下方式确定RCM点和传感标记的初始相对位置：利用所述转换关系、所述RCM点的起始位置和所述传感标记起始位置得到所述RCM点和所述传感标记在所述机械臂基坐标系中的初始相对位置。本实施例还可以通过以下方式确定传感标记的期望位置：根据所述RCM点在所述机械臂基坐标系中的相对位置变化量确定所述传感标记在所述机械臂基坐标系中的期望位置。

作为另一种可行的实施方式，本实施例可以通过以下方式确定RCM点和所述传感标记的初始相对位置：利用所述转换关系、所述RCM点的起始位置和所述传感标记起始位置得到所述RCM点和所述传感标记在所述光学传感器坐标系中的初始相对位置。本实施例还可以通过以下方式确定传感标记的期望位置：根据所述RCM点在所述光学传感器坐标系中的相对位置变化量确定所述传感标记在所述光学传感器坐标系中的期望位置。

进一步的，上述实施例可以通过以下方式实现床体跟随：判断所述床体或所述传感标记的位置是否发生变化；若是，则控制所述机械臂跟随所述床体或所述传感标记围绕所述RCM点运动。

具体的，本实施例可以利用所述光学传感器采集所述传感标记的当前位置；根据所述传感标记的当前位置计算所述传感标记在所述光学传感器坐标系中的相对位置变化量；判断所述传感标记在所述光学传感器坐标系中的相对位置变化量是否为零；若是，则判定所述床体或所述传感标记的位置未发生变化；若否，则判定所述床体或所述传感标记的位置发生变化。相应的，若传感标记的位置发生变化，可以根据所述传感标记在所述光学传感器坐标系中的相对位置变化量，计算所述RCM点的第一目标位置；其中，所述传感标记的当前位置和所述第一目标位置之间的相对位置为所述初始相对位置；控制所述机械臂执行跟随床体运动，以使所述RCM点运动至所述第一目标位置。

具体的，本实施例可以根据所述转换关系对所述传感标记在光学传感器坐标系中的相对位置变化量进行坐标转换，得到所述传感标记的在所述机械臂基坐标系中的相对位置变化量；根据所述传感标记的在所述机械臂基坐标系中的相对位置变化量计算所述RCM点在所述机械臂基坐标系中的第一目标位置；进而根据所述RCM点在所述机械臂基坐标系中的第一目标位置控制所述机械臂执行所述跟随床体运动。

进一步的，上述实施例可以通过以下方式实现RCM点的校验控制：在所述传感标记运动至所述期望位置之后，判断所述RCM点的位置是否发生变化；若是，则控制所述床体跟随所述RCM点运动。

具体的，本实施例可以利用获取所述RCM点的当前位置，根据所述RCM点的当前位置计算所述RCM点在所述机械臂基坐标系中的相对位置变化量；判断所述RCM点在所述机械臂基坐标系中的相对位置变化量是否为零；若是，则判定所述RCM点的位置未发生变化；若否，则判定所述RCM点的位置发生变化。若RCM点的位置发生变化，可以根据所述RCM点在机械臂基坐标系中的相对位置变化量，计算所述传感标记的第二目标位置；其中，所述RCM点的当前位置和所述第二目标位置之间的相对位置为所述初始相对位置；控制所述床体跟随机械臂运动，以使得所述传感标记运动至所述第二目标位置。

具体的，本实施例可以根据所述转换关系对所述RCM点在机械臂基坐标系中的相对位置变化量进行坐标转换，得到所述RCM点的在所述机械臂基坐标系中的相对位置变化量；根据所述RCM点的在所述机械臂基坐标系中的相对位置变化量计算所述传感标记在所述机械臂基坐标系中的第二目标位置；根据所述传感标记在所述机械臂基坐标系中的第二目标位置控制所述床体执行所述跟随机械臂运动。

下面通过在实际应用中的实施例说明上述实施例描述的流程。

在复杂机器人微创手术中，需要将手术床和手术机械臂进行术前匹配，实现二者坐标系一致，可以通过获取床体的偏移量，计算获得机械臂各个关节数据，完成机械臂跟随床体运动，保证跟随的位置和角度精确。为了提高机械臂RCM点和传感标记的坐标匹配精度，提高机械臂与手术床的随动运动精度，本申请提供一种通过机械臂远心不动点匹配坐标系的协同效验方法，包括以下步骤：

步骤1：根据机械臂基坐标系和光学传感器坐标系，在模型中建立统一的坐标系关系。

本实施例可以通过测量或标定的方式实现机械臂基坐标系和光学传感器坐标系转化，确定统一的坐标系。本实施例可以构建机器人的运动学模型，在模型中建立统一的坐标系，通过该方式可以根据RCM在机械臂坐标系的坐标，得到RCM点在光学传感器坐标系的坐标；同理，本实施例也可得到传感标记在机械臂基坐标系的坐标。

步骤2：离线采集机械臂RCM点在机械臂基坐标系中的起始位置，采集床体上传感标记在光学传感器坐标系中的初始位置，测量得到RCM点与传感标记的初始相对位置。

具体的，本实施例可以控制手术机器人的机械臂运动，使机械臂上的手术器械末端（RCM点）运动到手术床上方任意位置。作为一种可行的实施方式，本实施例可以通过尺子测量RCM点与光学传感器的相对位置关系，也可以通过正运动学计算得到RCM在光学传感器坐标系的位置。本实施例可以通过光学传感器采集得到传感标记的初始位置（该位置是摆位前期任意放置的传感标记位置）。在得到机械臂RCM点和传感标记的起始位置的基础上，可以得到RCM点与传感标记的初始相对位置。

步骤3：实时采集RCM点在机械臂基坐标系的当前位置。

为了确定RCM点与传感标记的起始相对位置，在测量RCM点与传感标记最初的摆位起始位置时，并未考虑患者放置在手术床时开孔位置，RCM点的起始位置（机械臂上器械末端）是任意放置，传感标记的起始位置也是随意放置的。当患者躺着手术床上后（不考虑床的姿态），机械臂摆位的过程是移动机械臂直至机械臂上手术器械末端的RCM点与患者开孔位置重合，通过采集此时（RCM点与患者开孔重合位置）机械臂关节数据/电机数据，利用正运动学得到RCM相对与机械臂坐标系的变化，即实现实时监测是RCM点的变化，得到RCM点相对机械臂坐标系的当前位置。本实施例可以通过移动RCM点达到患者开孔处，若传感标记不进行调整，将导致新的RCM点与传感标记的相对位置与之前发生偏移。

步骤4：将步骤1和2获得RCM点的当前位置和初始位置做差，得到RCM点当前位置与RCM初始位置（即，RCM点运动前后）的RCM相对位置变化量。

步骤5：将步骤3中得到RCM点的当前位置与RCM初始位置的运动前后RCM相对位置变化量，从机械臂基坐标系转化到光学传感器坐标系中，得到新的RCM点在光学传感器坐标系的变化量。

本步骤中的转化只考虑矩阵转化变化，不考虑位置变化。可以先绕Y轴旋转-105°，再绕Z轴旋转90°。具体的，本实施例可以根据RCM初始位置，已知RCM初始位置在机械臂坐标系的坐标，结合之前得到的RCM相对位置变化量，通过矩阵转化（两个坐标系的转化可采用现有的坐标转化方法），计算得到运动后的RCM在光学传感器坐标系的新的坐标。

步骤6：根据步骤2确定的RCM点与传感标记确定的初始相对位置，结合步骤5得到RCM点在光学传感器坐标系的变化量，通过计算得到传感标记的期望位置。

其中，在机械臂和光学传感器坐标系统一的基础上，可得到RCM初始位置在光学传感器的坐标系，同时步骤5已得到的RCM在光学传感器新的坐标和步骤4已得到RCM的变化量，为了保持新的RCM点与传感标记的相对位置不变，需要根据RCM点的运动，调整传感标记的运动至传感标记的期望位置，使得新的RCM点与传感标记的期望位置的相对位置保持不变。在执行完步骤6之后，初始的摆位程序均已完成。

步骤7：实时获取当前传感标记在光学传感器坐标系中的当前位置，同时获取光学传感器数据。根据传感标记在光学传感器坐标系中的当前位置相对于传感标记在光学传感器坐标系中的期望位置的相对位置变化量，与RCM相对位置变化量，建立传感标记与RCM的位置关系。

具体的，完成上述手术前的摆位程序后，当手术开始时，为了手术操作的需要，需要变化患者的位姿（即调整床的位姿），因传感标记安装在床上随床一起变化位姿，通过光学传感器实时监测传感标记的位置变化，得到传感标记与光学传感器的相对位置的变化量，从而得到传感标记在机械臂坐标系的变化量和坐标，为了保持RCM与传感标记的相对位置不变，通过计算得到RCM此时的期望位置，需要控制机械臂移动RCM点至RCM的期望位置。

步骤8：保证传感标记与RCM点相对位置关系固定，将传感标记和RCM相对位置变化量数据，由光学传感器坐标系转化到机械臂基坐标系。

本实施例可以根据RCM点与床下传感标记关系对床体或机械臂进行控制，进而保证床跟随机械臂移动的精准度。通本实施例过统一机械臂与光学传感器坐标系（先将机械臂坐标系转化到传感标记所在的光学传感器坐标系，再将传感标记所在的光学传感器坐标系转化到RCM所在的机械臂坐标系），确定机械臂上RCM点与传感标记之间的相对位置关系（BJ21023是通过标定得到，该方案是通过测量得到），监测床上安装的传感标记实时运动，根据传感标记实时位置变化数据控制机械臂跟随床运动，使得RCM点与床上安装的传感标记之间的相对位置保持不变。本实施例对RCM点与床体传感标记之间的相对位置偏移量进行效验，并根据效验结果，控制机械臂运动，使RCM点与床上传感标记之间的偏移量为最小。

上文中坐标系转化过程如下：

；公式（1）

；公式（2）

；公式（3）

保证：

，即可；

从而：

；公式（4）

或者

；公式（5）

式中

只考虑位置，不考虑旋转。

表示光学传感器坐标系与机械臂基坐标系相对位姿关系，只考虑旋转变换，位置变换不考虑。

为由光学传感器坐标系变换至机械臂基坐标系的3×3的旋转矩阵。上式中BASE表示机械臂基坐标系，MARK表示移动前的传感标记，MARK’表示移动后的传感标记，NDI为光学传感器坐标系，RCM表示移动前的远心不动点，RCM’ 表示移动后的远心不动点。

表示传感标记变化数据在光学传感器坐标系姿态“每个轴的旋转”；

表示传感标记移动前与光学传感器相对位姿；

表示传感标记移动后与光学传感器相对位姿；

表示RCM变化数据在机械臂基坐标系姿态；

表示机械臂基坐标系与移动前的RCM点相对位姿；

表示机械臂基坐标系与移动后的RCM点相对位姿；

表示传感标记变化数据在光学传感器坐标系姿态转换到机械臂基坐标系。

公式

，保证机械臂运动姿态和床的运动姿态一致。

公式（1）表示传感标记变化数据在光学传感器坐标系姿态“每个轴的旋转”；

公式（2）表示RCM变化数据在机械臂基坐标系姿态；

公式（3）表示传感标记变化数据在光学传感器坐标系姿态转换到机械臂基坐标系；通过公式(3)和上面的等式可以得到公式(4)。通过公式(1)和上面的等式可以得到公式(5)。

本实施例确定传感标记在床上和RCM点的起始位置，在工作过程中，主要实时监测是RCM点的位置变化（通过监测机械臂的关节/电机数据，和光学传感器与机械臂的姿态关系“没有X,Y,Z”），找到传感标记的期望位置；和实时监测传感标记的位置变化，找到RCM的期望位置。需要说明的是，本实施例只关注RCM点和传感标记的位置变化，不考虑二者的位姿变化。本实施例在手术前摆位的过程，对RCM和传感标记进行一次校准，同时在手术过程中，也可以根据RCM和传感标记的变化，相互校验彼此的运动。若发现其中一个位置存在偏移，及时进行校准，实现RCM和传感标记的相对位置始终保持不变，从而保障手术的安全。此过程，若RCM点存在侧向牵引力，通过力反馈计算，得到控制机械臂围绕此时RCM点的运动，实现纠正RCM存在的侧向牵引力。

本申请实施例还提供了一种机器人远心不动点的校验控制的相关装置，该相关装置包括存储介质和机器人。

本申请还提供了一种存储介质，其上存有计算机程序，该计算机程序被执行时可以实现上述实施例所提供的步骤。该存储介质可以包括：U盘、移动硬盘、只读存储器（Read-Only Memory ，ROM）、随机存取存储器（Random Access Memory ，RAM）、磁碟或者光盘等各种可以存储程序代码的介质。

本申请还提供了一种机器人，包括机械臂、底座、存储器和处理器，所述底座设置有光学传感器，所述存储器中存储有计算机程序，所述处理器调用所述存储器中的计算机程序时实现上述机器人远心不动点的校验控制方法执行的步骤。当然所述机器人还可以包括各种网络接口，电源等组件。

说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。应当指出，对于本技术领域的普通技术人员来说，在不脱离本申请原理的前提下，还可以对本申请进行若干改进和修饰，这些改进和修饰也落入本申请权利要求的保护范围内。

还需要说明的是，在本说明书中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的状况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

Claims

1.一种机器人远心不动点的校验控制方法，其特征在于，包括：

2.根据权利要求1所述机器人远心不动点的校验控制方法，其特征在于，在根据所述期望位置控制所述床体运动之后，还包括：

判断所述床体或所述传感标记的位置是否发生变化；

3.根据权利要求2所述机器人远心不动点的校验控制方法，其特征在于，判断所述床体或所述传感标记的位置是否发生变化，包括：

利用所述光学传感器采集所述传感标记的当前位置；

若是，则判定所述床体或所述传感标记的位置未发生变化；

若否，则判定所述床体或所述传感标记的位置发生变化。

4.根据权利要求3所述机器人远心不动点的校验控制方法，其特征在于，控制所述机械臂跟随所述床体或所述传感标记围绕所述RCM点运动，包括：

5.根据权利要求4所述机器人远心不动点的校验控制方法，其特征在于，根据所述传感标记在光学传感器坐标系中的相对位置变化量，计算所述RCM点的第一目标位置，包括：

相应的，控制所述机械臂执行跟随床体运动包括：

6.根据权利要求1所述机器人远心不动点的校验控制方法，其特征在于，在所述传感标记运动至所述期望位置之后，还包括：

判断所述RCM点的位置是否发生变化；

若是，则控制所述床体跟随所述RCM点运动。

7.根据权利要求6所述机器人远心不动点的校验控制方法，其特征在于，判断所述RCM点的位置是否发生变化，包括：

若是，则判定所述RCM点的位置未发生变化；

若否，则判定所述RCM点的位置发生变化。

8.根据权利要求7所述机器人远心不动点的校验控制方法，其特征在于，控制所述床体跟随所述RCM点运动，包括：

9.根据权利要求8所述机器人远心不动点的校验控制方法，其特征在于，根据所述RCM点在机械臂基坐标系中的相对位置变化量，计算所述传感标记的第二目标位置，包括：

相应的，控制所述床体执行跟随机械臂运动包括：

10.根据权利要求1所述机器人远心不动点的校验控制方法，其特征在于，在所述机械臂执行摆位操作时在线采集所述RCM点的当前位置，包括：

11.根据权利要求1所述机器人远心不动点的校验控制方法，其特征在于，在所述机械臂执行摆位操作时在线采集所述RCM点的当前位置，并根据所述RCM点的当前位置和所述RCM点的起始位置计算所述RCM点的相对位置变化量，包括：

12.根据权利要求1所述机器人远心不动点的校验控制方法，其特征在于，利用所述转换关系、所述RCM点的起始位置和所述传感标记起始位置得到所述RCM点和所述传感标记的初始相对位置，包括：

13.根据权利要求1所述机器人远心不动点的校验控制方法，其特征在于，利用所述转换关系、所述RCM点的起始位置和所述传感标记起始位置得到所述RCM点和所述传感标记的初始相对位置，包括：

14.一种机器人，其特征在于，包括机械臂、底座、存储器和处理器，所述底座设置有光学传感器，所述存储器中存储有计算机程序，所述处理器调用所述存储器中的计算机程序时实现如权利要求1至13任一项所述机器人远心不动点的校验控制方法的步骤。

15.一种存储介质，其特征在于，所述存储介质中存储有计算机可执行指令，所述计算机可执行指令被处理器加载并执行时，实现如权利要求1至13任一项所述机器人远心不动点的校验控制方法的步骤。