一种工程机械臂的在线标定方法与流程

本发明涉及工程机械领域，尤其涉及一种工程机械臂的在线标定方法。

背景技术：

利用激光跟踪仪的标定方法应用在离线标定系统中时，激光跟踪仪一般安装在机械臂前方，而靶球安装在机械臂末端，以保证激光跟踪仪能时刻检测到靶球位置。而进行在线标定机械臂工作时，大多是将激光跟踪仪架设在地上，且对激光跟踪仪的架设位置的选择有一定的要求，以保证靶球不被遮挡，因此每次进行标定时，都需人工对激光跟踪仪进行重新安装以及设站，测量标定效率较低。

技术实现要素：

本发明所要解决的技术问题之一是需要提供一种适用于在线标定的且标定效率高、准确度较好的工程机械臂标定方法。

为了解决上述技术问题，本申请的实施例首先提供了一种工程机械臂的在线标定方法，该方法包括：标靶安装步骤，将至少三个标靶安装在所述工程机械臂的末端，使这至少三个标靶与该机械臂臂架的中轴线的设定点成预定位置关系；机械臂末端位姿计算步骤，获取所述至少三个标靶的位置信息、所述标靶的结构尺寸参数和安装尺寸参数、以及机械臂各关节的运动量值，利用所述预定位置关系和预设的理想运动学模型，计算得到该机械臂末端的位姿实测值和理论值；模型修正步骤，对比该机械臂末端的位姿实测值和理论值，根据对比结果结合预先构建的误差模型来修正所述理想运动学模型，得到修正后的运动学模型。

优选地，还包括：根据所述机械臂结构中的臂架连杆dh几何参数来构建误差模型。

优选地，在安装三个标靶时，所述预定位置关系为三个标靶以机械臂臂架的中轴线为中心，相邻两标靶与中轴线上的设定点之间连线彼此相差设定角度。

优选地，在所述机械臂末端位姿计算步骤中，包括如下步骤：实测姿态计算步骤，确定标靶坐标系相对于末端臂架坐标系的第一姿态矩阵，以及所述标靶坐标系相对于机械臂载体坐标系的第二姿态矩阵，根据第一姿态矩阵和第二姿态矩阵确定末端臂架坐标系相对于机械臂载体坐标系的姿态矩阵，从而得到该机械臂末端的姿态信息。

优选地，在所述机械臂末端位姿计算步骤中，还包括如下步骤：实测位置计算步骤，根据标靶的结构尺寸参数以及安装尺寸参数，分别获取机械臂中轴线上的设定点和末端臂架关节中心点的距离以及该设定点在机械臂载体坐标系下的坐标，进而获取末端臂架关节中心点在机械臂载体坐标系下的坐标。

优选地，在实测姿态计算步骤中，根据三个标靶在机械臂载体坐标系下的坐标以及标靶坐标系与三个标靶之间的位置关系可求得所述第一姿态矩阵；根据获取的所述至少三个标靶的位置信息以及这至少三个标靶与该机械臂臂架的中轴线的设定点构成预定位置关系确定所述第二姿态矩阵。

优选地，在所述机械臂末端位姿计算步骤中，还包括如下步骤：理论值计算步骤，将获取的机械臂各关节的运动量值代入所述理想运动学模型中，求得机械臂末端在理想状态下的位置和姿态。

优选地，还包括：工况变化判断步骤，判断机械臂载体的工况是否发生变化，若发生变化，则将当前修正后的运动学模型作为理想运动学模型，并重新执行机械臂末端位姿计算步骤和模型修正步骤。

优选地，根据实际施工强度设定一个标定周期，每经过一个标定周期对机械臂进行一次重新标定。

与现有技术相比，上述方案中的一个或多个实施例可以具有如下优点或有益效果：

1、将激光跟踪仪安放在台车固定处，在进行标定时，无需每次都对激光跟踪仪进行设站，简化测量过程，提高测量效率。

2、将至少三个可折叠的靶球结构安装在机械臂末端，根据这些靶球与该末端臂架的中轴线上的设定点(一般为末端臂架的尾端在中轴线上的点，后面可简称中心点)的位置关系，结合获取的靶球的位姿信息，能够得到末端臂架相对于机械臂载体坐标的位姿信息(位姿实测值)，将这些数值与通过理想运动学模型得到的理论值进行比对，修正该理想运动学模型，从而完成对该机械臂的标定，提高了标定的准确度。

3、根据挠度变形情况对施工面进行预分区，然后在分区内均匀的选取测量点，进行标定，从而起到标定效果，提高标定效率和运动学模型的准确性，改善施工质量。

本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明的技术方案而了解。本发明的目的和其他优点可通过在说明书、权利要求书以及附图中所特别指出的结构和/或流程来实现和获得。

附图说明

附图用来提供对本申请的技术方案或现有技术的进一步理解，并且构成说明书的一部分。其中，表达本申请实施例的附图与本申请的实施例一起用于解释本申请的技术方案，但并不构成对本申请技术方案的限制。

图1为本申请的工程机械臂的在线标定方法的示例一的流程示意图。

图2为本申请的一个例子中靶球相对于机械臂中轴线上的设定点(简称中心点)a’安装的安装示意图。

图3为本申请的一个例子中建立的标靶、标靶坐标系以及末端臂架关节坐标系之间的机械臂载体坐标系位姿关系示意图。

图4为本申请的一个例子中的靶球机构的示意图。

图5为图4所示的靶球机构展开的运动简图。

图6为本申请的一个例子的计算末端臂架位置的简图。

图7为本申请的由臂架挠度变形导致的末端位置的变化示意图。

图8为本申请的工程机械臂的在线标定方法的示例二的流程示意图。

图9为本申请的工程机械臂的在线标定方法的计算框图。

具体实施方式

以下将结合附图及实施例来详细说明本发明的实施方式，借此对本发明如何应用技术手段来解决技术问题，并达成相应技术效果的实现过程能充分理解并据以实施。本申请实施例以及实施例中的各个特征，在不相冲突前提下可以相互结合，所形成的技术方案均在本发明的保护范围之内。

本发明实施例的技术原理简要概括如下：将至少三个可折叠的靶球结构安装在机械臂末端，根据这些靶球与该末端臂架的中轴线上的设定点(一般为末端臂架的尾端在中轴线上的点，后面可简称中心点)的位置关系，结合获取的靶球的位姿信息，能够得到末端臂架相对于机械臂载体坐标的位姿信息(位姿实测值)，将这些数值与通过理想运动学模型得到的理论值进行比对，修正该理想运动学模型，从而完成对该机械臂的标定。为了进一步了解本发明，下面说明本申请涉及到的几个实施例。

第一实施例

首先，介绍以下本例中用到的测量工具：激光跟踪仪(坐标信息采集器的一个例子)、可折叠靶球机构(标靶的一个例子，具体结构可如图4所示)。该可折叠靶球机构能够通过电机的驱动进行展开和闭合，以保证在标定过程中，防止出现由于机械臂的运动导致的靶球被遮挡的情况，保证测量标定的正常进行。有关该靶球机构的结构，此处不做限定，只要能够具备上述功能的靶球均可。

如图4所示，在附图中各附图标记的含义如下：在附图中各附图标记的含义如下：1、底座，2、可折叠式靶球，3、保护板，10、靶球支撑卡座，11、保护板支撑卡座，12、中间传动架，13、齿条，14、第一滑槽，15、第二滑槽，16、第一滑动销，17、第二滑动销，18、电机，19、丝杠，21、翻转臂，22、支撑臂，23、靶球本体，31、回转轴，32、齿轮。

下面介绍一下该靶球机构的结构。

如图4所示，本实施例提供了一种靶球机构，包括：底座1，设置在所述底座1上的可折叠式靶球2，所述可折叠式靶球2构造成能够所述底座1上折叠或伸展。

优选地，所述底座1设置有可折叠的保护板3，所述可折叠式靶球2处于折叠状态时，所述保护板3处于折叠状态并覆盖在所述可折叠式靶球2的上方。优选地，所述底座1包括连接所述可折叠式靶球2的靶球支撑卡座10，以及连接所述保护板3的保护板支撑卡座11；所述靶球支撑卡座10和所述保护板支撑卡座11之间设置有可移动的中间传动架12；其中，所述中间传动架12移动时能够带动所述可折叠式靶球2和所述保护板3同时折叠或伸展。

在一个例子中，靶球支撑卡座10上设置有第一滑槽14，中间传动架12的一端设置有与所述第一滑槽14相配合的第一滑动销16；保护板支撑卡座11上设置有第二滑槽15，所述中间传动架12的另一端设置有与第二滑槽15相配合的第二滑动销17。

进一步，可折叠式靶球2包括铰接在靶球支撑卡座10上的翻转臂21，以及铰接在所述中间传动架12上的支撑臂22，并且所述支撑臂22的上端设置有靶球本体23；其中，翻转臂21的上端与支撑臂22的中部铰接。

在一个例子中，靶球本体23与所述支撑臂22之间设置一定角度的夹角，所述夹角保证在所述可折叠式靶球2处于折叠状态时所述靶球本体2与所述翻转臂21处于平行状态。

优选地，保护板3的底部通过回转轴31与所述保护板支撑卡座11铰接，所述回转轴31上设置与随所述回转轴31共同转动的齿轮32；所述中间传动架12上设置有与所述齿轮32相配合的齿条13。

在一个例子中，回转轴31上设置有使所述保护板3在不受力的情况下处于伸展状态的扭簧，并且所述保护板3在伸展状态时，所述齿条13随所述中间传动架12移动到与所述齿轮32脱离的位置。

优选地，所述保护板支撑卡座11与所述中间传动架12之间设置有动力装置，所述动力装置构造成能够驱动所述中间传动架12移动从而带动所述可折叠式靶球2和所述保护板3折叠或伸展。

进一步，动力装置包括丝杠19，所述丝杠19的一端转动连接在所述保护板支撑卡座(11)上，另一端连接有带动丝杠19转动的电机18；其中，中间传动架12上设置有螺孔，所述丝杠19穿过所述螺孔，并且所述丝杠19的螺纹与所述螺孔的螺纹相配合。

接着，说明在执行在线标定前的准备工作：

1、根据已知机械臂载体的几何设计参数，利用建模软件，在线生成机器人的理想运动学模型，并以几何模型在软件中呈现。

2、根据机械臂结构建立误差模型，具体是根据机械臂结构中的臂架连杆dh几何参数来构建误差模型，连杆i的误差模型即为后面的式(1-3)，然后将各连杆的误差叠加即得式(1-9)为总误差。

接下来，参考图1来说明如何进行工程机械臂的在线标定，图1为本申请的工程机械臂的在线标定方法的示例一的流程示意图。

在步骤s110(标靶安装步骤)中，将至少三个标靶安装在所述工程机械臂的末端，使这至少三个标靶与该机械臂臂架的中轴线上的设定点成预定位置关系。

在步骤s120(机械臂末端位姿计算步骤)中，获取所述至少三个标靶的位置信息、所述标靶的结构尺寸参数和安装尺寸参数、以及机械臂各关节的运动量值，利用所述预定位置关系和预设的理想运动学模型，计算得到该机械臂末端的位姿实测值和理论值。各关节的运动量值包括转动关节的转动角以及移动关节的移动量。

在步骤s130(模型修正步骤)中，对比该机械臂末端的位姿实测值和理论值，根据对比结果结合预先构建的误差模型来修正所述理想运动学模型，得到修正后的运动学模型。

下面以在机械臂末端安装三个可折叠靶球机构为例，具体说明上述各个步骤。

在步骤s110中，将三个靶球机构以机械臂臂架的中轴线为中心，使得相邻两靶球机构中轴面(该面是指相邻两靶球与中轴线上的设定点构成的面，该例中的设定点为中心点a’，如图2中的平面p1、p2和p3)之间彼此相差设定角度，例如120°对称安装，如图2所示，图中b、c、d分别为三个靶球机构，a’为臂架中轴线上的中心点。

在步骤s120中开始执行标定：首先，工控机发出标定指令，控制电机使三个靶球展开，激光跟踪仪启动。然后，利用激光跟踪仪采集末端三个靶球坐标信息，并通过can总线反馈到工控机中。接着，工控机利用反馈的三个靶球的位置信息，求出机械臂末端位置和姿态。下面列举一示例来说明如何通过三靶球测量机械臂的位置姿态的，具体包括姿态测量和位置测量两部分。

(1)姿态测量

确定标靶坐标系相对于末端臂架坐标系的第一姿态矩阵，以及所述标靶坐标系相对于机械臂载体坐标系的第二姿态矩阵，根据第一姿态矩阵和第二姿态矩阵确定末端臂架坐标系相对于机械臂载体坐标系的姿态矩阵，从而得到该机械臂末端的姿态信息。具体方式如下所述。

首先，将靶球相对于机械臂中心点a’安装如图2所示。由于靶球中心b、c、d三点正好构成一个三角形，因此当b、c、d位置确定时，在机械臂中轴线上必然存在唯一点a，使得两两之间相互垂直。

为了便于计算由三个靶球构造而成空间参考对象的位姿，以a为原点，方向分别为x、y、z轴方向来建立靶球坐标系a-xayaza，同时在臂架ea’上建立臂架关节坐标系e-xeyeze，e为末端臂架的关节中心点，xe轴沿臂架中轴线，与同向，ze轴垂直臂架向上，与方向相同；建立机械臂载体坐标系o-xyz如图3所示。根据靶球结构特点可知，在靶球展开和收回过程中，靶球中心b、c、d相对于臂架中心点a’仅距离发生改变，方向不变。因此无论靶球运动到任何位置，坐标系a-xayaza与末端臂架关节坐标系e-xeyeze存在着确定的姿态矩阵(第一姿态矩阵)。

将投影到xaaya平面得到与xa轴的夹角为α＝45°，与夹角为β＝35.26°，根据欧拉角定义可求的靶球坐标系a-xayaza与末端臂架坐标系e-xeyeze存在着确定的姿态矩阵

因此只需求得靶球坐标系a-xayaza相对于机械臂载体坐标系的姿态矩阵即可求得末端臂架相对于机械臂载体坐标系的姿态矩阵

根据获取的所述至少三个标靶的位置信息以及这至少三个标靶与该机械臂臂架的中轴线的设定点构成预定位置关系确定所述第二姿态矩阵。具体地，利用激光跟踪仪可测的某位置下靶球b、c、d在机械臂载体坐标系下的坐标分别为(^oxb,^oyb,^ozb)、(^oxc,^oyc,^ozc)、(^oxd,^oyd,^ozd)，则a′在机械臂载体坐标系下的坐标为由于a-bcd正好构成了一个以a顶点的特殊三棱锥：三条棱长度相同且相互垂直，因此在已知b、c、d情况下，利用几何数学计算可求得a点的坐标。a点在机械臂载体坐标系下的坐标(^oxa,^oya,^oza)应满足以下关系式：

根据姿态矩阵定义可知，姿态矩阵为靶球坐标系xa、ya、za轴上的单位向量在机械臂载体坐标系x、y、z轴下的投影。由a、b、c、d点坐标可求得xa、ya、za轴上的单位向量：

则靶球坐标系a-xayaza相对于机械臂载体坐标系的姿态矩阵(第二姿态矩阵)为：

则末端臂架关节坐标系相对于机械臂载体坐标系的姿态矩阵为：

(2)位置测量

根据标靶的结构尺寸参数和安装尺寸参数，分别获取机械臂中轴线上的设定点和末端臂架关节中心点的距离以及该设定点在机械臂载体坐标系下的坐标进而获取末端臂架关节中心点在机械臂载体坐标系下的坐标。

图5为靶球机构展开的运动简图，图中轴线a为末端臂架的中轴线，转动副g为翻转臂4与靶球支承卡座5之间的回转轴；转动副f为支撑臂3与翻转臂4之间的回转轴；b为靶球测量中心点；移动副h对应于活动销轴6。

如图6所示，根据靶球结构尺寸，可确定的尺寸参数有：连杆gf的长度dgf；bh之间的距离dbh；以及初始状态下g和h之间的水平距离dghx；在移动关节h处安装位移传感器，测量活动销轴6的位移量δd(利用位移传感器检测)；eg’之间的距离为d1；移动关节h距臂架中轴线距离为da′i；转动副g距臂架中轴线距离为dgg′。根据上述参数，进一步可求得以下尺寸：

g和h的实际水平距离：

d2＝dghx+δd

根据b点与a′点的坐标可求得靶球距臂架中轴线距离因此h与j的水平距离：

根据a点与a′点的坐标可求得两点之间的距离而为：

由于与共线，因此：

则可求得即为e点在机械臂载体坐标系下的坐标：

另一方面，还需要计算理想状态下的机械臂末端的位置和姿态。具体地，利用机械臂的关节传感器检测此末端坐标下各关节的运动量值，并通过can总线反馈到工控机中。工控机将反馈的各关节的运动量值代入运动学模型中，求得机械臂末端在理想状态下的位置和姿态，对比实测位姿与运动学模型求的位姿。

最后，将对比结果代入到误差模型中，并结合理想运动学模型，得到各臂架尺寸的修正值，得到修正的运动学模型，进行重新建模。

下面具体说明如何得到修正后的运动学模型。

根据设计要求，可知臂架连杆i理论上的dh几何参数，具体包括关节转角θi，关节扭角αi、关节连杆长度ai、关节偏置距离di。由上述参数可建立机械臂的理论运动学模型，且其齐次变换矩阵为(理论运动学模型的一种表达)，表达如下：

定义沿x,y,z轴移动的微分移动量分别为dx,dy,dz，绕x,y,z轴转动的微分角度分别为δx,δy,δz。当臂架连杆i坐标系做(dx,dy,dz)平移微分运动以及(δx,δy,δz)的旋转微分运动时，其对应变换式为：

臂架连杆i坐标系做如上的微分运动后，对应末端臂架位姿的微分变化即连杆i的误差模型为：

其中，为臂架连杆i坐标系到末端臂架n的齐次变换矩阵，δi为微分算子：

其中，i为单位矩阵。

假设臂架连杆i的几何参数误差δxi＝[δαi,δai,δθi,δdi]，则该坐标系i理论和实际的位姿误差为：

因此可得：

因此可得臂架连杆i坐标系的几何参数误差与臂架连杆坐标系i的微分运动之间的关系。

则由机械臂连杆i的几何尺寸误差产生的末端臂架坐标系n的总误差为：

其中，tc为末端臂架坐标系的测量位姿，t为末端臂架的理论位姿。

针对n自由度的机械臂，其待标定的参数有4n个。根据待标定参数数目确定需要采集点的数量，实际为了提高标定的鲁棒性，一般选取运动轨迹中的50个点进行标定。将50个点位姿信息代入(1-9)中，此时可获得大于待标定参数个数的方程数，因此利用lm算法(levenberg-marquardt算法)对待标定参数进行拟合，得到各个臂架的几何参数误差的最优值δxi＝[δαi,δai,δθi,δdi]。则修正后的机械臂dh几何参数以及修正的运动学模型为：

^modifyxi＝[αi+δαi,ai+δai,θi+δθi,di+δdi](1-10)

参考流程框图9，一个具体的示例包括如下步骤：

步骤1：在规划的运动轨迹中选取50个点，测量50个末端臂架点的位姿(为实测值)，以及各个点对应的不同关节的运动量，将该运动量值带入理想运动学模型中得到该机械臂末端的位姿理论值；

步骤2：将50个点代入式(1-9)中，利用lm算法对待标定参数进行拟合，得到偏差最小情况下的，即最优的微分算子解δi(1≤i≤n)。

步骤3：利用最优的微分算子解δi(1≤i≤n)代入式(1-8)即可求得此时δxi(1≤i≤n)。

步骤4：将δxi(1≤i≤n)代入式(1-10)，即可求得修正后的机械臂dh几何参数。

需要说明的是，本领域技术人员可以根据实际需求来设定偏差值，具体数值此处不做限定。通过上述步骤，能够使得标定后的运动学模型更加准确，通过该模型得到的机械臂位姿信息更加精确。

第二实施例

图8为本申请的工程机械臂的在线标定方法的示例二的流程示意图。该实施例是在第一实施例的基础上，进一步改进的标定方法。在图中，将与第一实施例不同的步骤用不同的标号表示。

在步骤s110(标靶安装步骤)中，将至少三个标靶安装在所述工程机械臂的末端，使这至少三个标靶与该机械臂臂架的中轴线上的设定点成预定位置关系。

在步骤s120(机械臂末端位姿计算步骤)中，获取所述至少三个标靶的位置信息、所述标靶的结构尺寸参数和安装尺寸参数、以及机械臂各关节的运动量值，利用所述预定位置关系和预设的理想运动学模型，计算得到该机械臂末端的位姿实测值和理论值。

在步骤s130(模型修正步骤)中，对比该机械臂末端的位姿实测值和理论值，根据对比结果结合预先构建的误差模型来修正所述理想运动学模型，得到修正后的运动学模型。

在执行完步骤s130之后，还执行步骤s310(工况变化判断步骤)，判断机械臂载体的工况是否发生变化，若发生变化，则将当前修正后的运动学模型作为理想运动学模型，并重新执行机械臂末端位姿计算步骤和模型修正步骤，即返回执行步骤s120和s130等步骤。

需要说明的，在本例中，所述工况变化指的是针对同一施工面，台车的施工区域变化。由于为使机械臂达到不同位置的施工区域，臂架姿态情况是不同的，考虑到一个臂架挠度变形方向始终不变且竖直向下的，因此当此臂架发生姿态变化时，臂架挠度变形与臂架的位置关系就会发生变化，相应运动学模型也就不再准确。因此需根据实际臂架姿态变化特点以及挠度变形情况对施工区域进行适当的划分，并针对不同施工区域建立并采用相应的修正的运动学模型。

举个例子，如图7a所示，假设有一个水平臂架ob，且沿x轴方向，则其考虑挠度变形后其臂架建模得到修正的实际末端位置为b’，当b绕x轴转动90°后，如果模型不变，则其末端位置修正位置为b’，如图7b所示，但是根据实际情况可知在考虑挠度变形情况下，其臂架实际末端位置应该是在b”点，如图7c所示。

因此，本实施例通过增加了判断工况变化的步骤，根据不同工况建立与之相符的运动学模型能够提高模型的精度，进而提高机械臂的标定准确性。

其他实施例

另外，在持续且高强度的施工过程，台车机械臂不可避免会发生各种磨损以及变形等变化，此类变化属于不可恢复性变化，将直接导致运动学模型不准确。考虑到此类变化为渐变且缓慢的，因此可根据实际施工强度设定一个标定周期t，即系统每经过一个标定周期t就对机械臂进行一次重新标定，自动对修正模型进行复核，即每隔一个标定周期执行第一实施例或第二实施例的各个步骤，此处不再赘述。有关具体周期，本发明不做限定，技术人员可以根据需要进行设定。

虽然本发明所揭露的实施方式如上，但所述的内容只是为了便于理解本发明而采用的实施方式，并非用以限定本发明。任何本发明所属技术领域内的技术人员，在不脱离本发明所揭露的精神和范围的前提下，可以在实施的形式上及细节上作任何的修改与变化，但本发明的专利保护范围，仍须以所附的权利要求书所界定的范围为准。