[发明专利]一种基于拉线编码器的站点可动式测量机构的初始位置校准方法

摘要

一种基于拉线编码器的站点可动式测量机构的初始位置校准方法，为形象化四站式布局，对总体的测量结构进行初步建模；将表示机器人各个关节之间坐标系的D‑H模型推广到测量模块四站式布局站点之间的坐标变换，并且引进α、d1、d2、β四个参数来表示四个站点之间的坐标变换；建立四站坐标系数学模型；对上述测量结构建立的D‑H模型进行求解，得出初始位置校准几何模型中的四个未知量，从而确定各个站点之间的相互位置关系，确保测量机构初始位置的精度。该算法效率高，成本较低并且环境适应性强，应用范围广，易于实现，操作方便。

主权项

1.一种基于拉线编码器的站点可动式测量机构的初始位置校准方法，其特征是它采用站点可动式测量机构，它包括两个固定装置(1)，各固定装置(1)上安装有两个拉线编码器，即测量机构的两个站点，两个固定装置(1)上的两个站点之间的位置固定；该校准方法包括以下步骤：S1、对可动式测量机构的四个站点进行初步建模：设下左、下右、上左、上右四个站点编号分别为站点1—站点4，设站点1为参考坐标系T1，站点2的坐标系为T2，站点3的坐标系为T3，站点4的坐标系为T4，站点1—站点4的基准点分别表示各站点滑轮的下切点，记为O1‑O4，以站点1的基准点O1指向站点2的基准点O2作为参考坐标系的X1轴，以站点1的基准点O1指向站点3的基准点O3作为参考坐标系的Y1轴，将以基准点O1为原点，垂直于X1轴、Y1轴所在平面的直线作为Z1轴；S2、对四站式布局的站点1‑4建立D‑H模型，站点2与站点1固连，站点2、3、4的坐标系与站点1之间的关系通过齐次变换矩阵来表示，具体的变换顺序如下：站点2：站点2的坐标系方向与参考坐标系T1一致，原点为站点2的基准点，因此通过参考坐标系沿着X轴平移固定距离a得到；站点3：由于Y1轴与Y3轴共面，将参考坐标系绕Z1旋转α角使得Y1轴、Y3轴平行，然后，将旋转α角后的坐标系沿着旋转后的X1轴平移距离d1使得Y1轴、Y3轴共线；再将变换后的坐标系沿着Y1(Y3)轴平移距离d2使得平移d1后的坐标系与站点3坐标系T3的基准点O3重合；最后，再将平移d2后的坐标系绕Y1(Y3)轴旋转β角使得X1轴，X3轴共线即可得到坐标系T3；站点4：站点4的坐标系方向与坐标系T3一致，原点为站点4的基准点，因此通过坐标系T3沿着X轴平移固定距离a得到；具体变换公式如下：T2＝Trans(a,0,0)T3＝R(Z1,α)·Trans(d1,0,0)·Trans(0,d2,0)·R(Y1,β)T4＝R(Z1,α)·Trans(d1,0,0)·Trans(0,d2,0)·R(Y1,β)·Trans(a,0,0)其中：a表示站点1基准点O1与站点2基准点O2的距离即站点3基准点O3与站点4基准点O4的距离；α,d1,d2,β根据第二个固定装置的放置位置获取；S3、设置校准点M1、M2：在参考坐标系T1选择任意一点作为校准点M1，在参考坐标系T4选择任意一点作为校准点M2，具体如下；采用一组拉线编码器获取站点3、站点4与校准点M1的拉线长度q3,q4；采用另一组拉线编码器获取站点1、站点2与校准点M2的拉线长度q1,q2；站点3与站点4拉线末端要相交于站点1的校准点M1，在参考坐标系T1下M1的坐标为M1，站点1与站点2拉线末端要相交于站点4的校准点M2，在参考坐标系T1下校准点M2的坐标M2，采用下述公式表达：M2＝TAM23＝R(Z1,α)Trans(d1,0,0)Trans(0,d2,0)M23其中：TA表示将站点3相对于参考坐标系即Т1的变换矩阵，M23表示校准点M 2在坐标系T 3上的坐标位置，由于T3到T4的变换矩阵是已知的—Trans(a,0,0),所以，M2的坐标应该包括Trans(a,0,0)﹒M2在站点4坐标系下的坐标，我们把这两项的乘积合成一个坐标M23；S4、获取四个站点滑轮上切点的坐标Pi,i＝1,2,3,4；S4‑1、获取四个站点的基准点O1‑O4即滑轮下切点相对于参考坐标系T1的坐标，将站点4基准点投影到前三个站点基准点构成的平面上，引入的误差符合工程应用的需求，将站点3投影在前述平面上相当于忽略β角对站点4坐标系Z坐标的影响；O1(0,0),O2(a,0),O3＝TAO1＝R(Z1,α)Trans(d1,0,0)Trans(0,d2,0)O1,O4＝TATBO1＝TAO2＝R(Z1,α)Trans(d1,0,0)Trans(0,d2,0)O2其中，a表示站点1基准点O1与站点2基准点O2的距离即站点3基准点O3与站点4基准点O4的距离；TA表示将站点3相对于参考坐标系即Т1 的变换矩阵，TB表示将站点2相对于参考坐标系即Т1 的变换矩阵；S4‑2、采用下述公式获取四个站点滑轮的圆心坐标Q1‑Q4；Q1(0,r),Q2(a,r),Q3＝TAQ33,Q4＝TATBQ33＝TAQ43其中，r表示滑轮半径，Q33表示Q3在坐标系T 3下的坐标，Q43表示Q4在坐标系T 3下的坐标；S4‑3、采用下述公式获取四个站点滑轮的上切点坐标Pi,i＝1,2,3,4，其中：i表示站点编号，T_i表示站点i所在的坐标系，Tran(x,y)表示将站点坐标系平移到以滑轮的圆心为坐标原点的坐标系的平移矩阵，E_i表示滑轮的圆外任意一点在以滑轮圆心为坐标系原点的坐标系下的坐标，Q点表示每个滑轮的圆心，QE表示校准点与站点i所在的坐标系的距离，站点3、站点4选取校准点M1，站点1、站点2选取校准点M2，表示旋转角度的旋转矩阵，表示滑轮圆心与圆外一点E连接与滑轮的交点F在以滑轮为圆心坐标系下的坐标；S5、根据步骤S4获取的上切点坐标，计算四个拉线弧长在滑轮上的包络角度αi,i＝1,2,3,4，αi表示滑轮上切点P、滑轮外一点E与滑轮圆心Q的夹角；S6、将前述计算结果带入下述公式获取α,d1,d2；其中，q₁,q₂表示拉线编码器获取的站点1、站点2与校准点M2的拉线长度，q₃,q₄表示拉线编码器获取的站点3、站点4与校准点M1的拉线长度；表示在参考坐标系下各点之间的长度，α₁,α₂,α₃,α₄表示拉线编码器的四个拉线弧长在滑轮上的包络角度；S7、采用拉线编码器对任一点进行检测，获取该检测点到四个站点的四条拉线长度L1、L2、L3、L4，采用下述公式获取β，完成四个站点坐标系的建立，从而完成校准；其中：x4,y4,z4分别表示站点4的坐标，其只跟β有关，x,y,z分别表示被测点的坐标，l4表示拉线编码器采集到的被测点与站点4的距离。