基于机身姿态角度校正的六足机器人关节角度标定方法与流程

本发明涉及足式移动机器人运动情况的标定方法，属于机器人技术领域，具体地，涉及一种基于机身姿态角度校正的六足机器人关节角度标定方法。

背景技术：

相较于轮式移动机器人，足式移动机器人因其具有灵活多变的运动模式，故拥有较强的越障能力，在非结构化环境中具备更好的运动功能鲁棒性。作为足式移动机器人的典型代表，六足移动机器人正逐渐成为足式移动机器人领域研究的热点。

由于六足移动机器人多属于混联机构，运动冗余关节较多，因此其运动控制的准确性及效率难以保证，运动学问题也就成为了六足移动机器人控制理论研究领域的主要热点之一。六足移动机器人的运动精度因其结构特征主要取决于足部各支链的协调与运动轨迹精度，并需要尽量减小足底受力对各关节角度的控制精度影响。因此需要准确的矫正各关节转动角度，从而提高运动轨迹控制精度，降低足底与地面的接触冲力。

经检索：

专利号为201220392380，名为“一种工业机器人的机械零点标定装置”，该发明公开了一种设置关节机械零点标定块的标定方法，该新型专利通过高精度加工和装配在标定关节处设置机械零点标定机构，进而实现零位标定。

专利号为201410401317，名为“一种六关节工业机器人的精度标定装置”，该发明公开了一种在机器人末端连接固定光源装置进行标定的方法，该方法利用激光笔和幕布进行快速低成本标定。

专利号为201610188161，名为“仿爬行类四足步行机器人运动参数的标定”，该发明公开了一种仿爬行四足步行机器人运动参数的标定装置，该方法建立专用的标定装置，利用测量平台测量机器人的运动轨迹进行运动参数的标定。

但是上述专利存在以下不足：标定过程需要使用标准标定块，或者使用特制的标定平台，无法避免的机械制造误差将对标定过程产生误差累计，无法保证机械零点标定的准确性。

技术实现要素：

针对现有技术中的缺陷，本发明的目的是提供一种基于机身姿态角度校正的六足机器人关节角度标定方法，具有较高的精度和实时性，能快速准确地完成对多足移动机器人关节角度的校正，从而保证机器人多足协调与运动轨迹精度。

为实现以上目的，本发明提供一种基于机身姿态角度校正的六足机器人关节角度标定方法，即：

驱动六足移动机器人以任意三条支撑足支撑地面，选取其中第一条支撑足并驱动该第一条支撑足上某一关节发生角度转动，且该支撑足的足底滑动；第二条和第三条支撑足的足底与地形不滑动，保持第二条和第三条支撑足固定不动；

建立关节角度与姿态间的函数关系，获得关于关节零点的线性方程组，求解即解出第一条支撑足的各关节角度，从而实现对六足机器人的全部关节角度标定。

所述的六足机器人，其六个结构相同的支撑足绕正六边形机身呈旋转对称布置；从六足机器人机身到支撑足足底上的三个关节依次定义为：髋关节、臀关节、膝关节；

在六足机器人机身的重心处设立机身坐标系，其中：机身坐标系的xoy平面与机身平面平行，机身坐标系的z轴垂直于机身平面；六足髋关节的转动轴垂直于机身平面，每个支撑足上的臀关节转动轴与膝关节转动轴相互平行，且均与机身平面平行。

具体的，所述方法包括如下步骤：

步骤1、初始化六足机器人机身的imu传感器的三自由度陀螺角度；

步骤2、使某一支撑足任意关节旋转给定角度；

步骤3、读取imu传感器中陀螺三自由度角度；

步骤4、使步骤2中该关节再次重复步骤2旋转动作，重复步骤3，直至得到五组姿态imu传感器中陀螺三自由度角度数据；

步骤5、若步骤4得到的五组数据不全相同，则转到步骤6进行求解；

若步骤4得到的五组姿态数据有两组或多组是相同的，则转动步骤2中该支撑足的任意一个关节一定角度，但该关节不得是步骤2中已旋转过的关节，重复步骤2～4，直至得到五组不全相同的姿态imu传感器中陀螺三自由度角度姿态数据；

步骤6、根据支撑足足底相对机身坐标系的坐标，求解六足机器人步骤2中进行旋转运动的支撑足的关节角度；

步骤7、重复以上操作，即对六足机器人六个支撑足进行关节角度标定。

优选地，步骤1中，利用imu传感器的三自由度陀螺感知六足机器人机身姿态角度；初始化imu传感器，是通过imu传感器内部的地磁偏角传感器进行校正。

优选地，步骤1中，六足机器人支撑至少需要三条支撑足，考虑机器人内的三条支撑足机构，设支撑足底到机身重心位置矢量分别为p1c、p2c、p3c，简化为：pic＝(xic,yic,zic)^t(i＝1,2,3)；由权利要求书中公式(6)、(7)、(8)：

xi＝(l1+l2cosβi+l3cosθi)sinαi(6)

yi＝(l1+l2cosβi+l3cosθi)cosαi(7)

zi＝l2sinβi+l3sinθi(8)

其中；αi表示第i个髋关节相对与零点的转动角度，βi表示第i个臀关节相对于零点的转动角度，γi表示第i个膝关节相对于零点的转动角度，l1为髋关节相对于机身重心距离，l2为大腿机构长度，l3为小腿结构长度；

由权利要求书中公式(9)、(10)、(11)得：

xic＝xiα+xi(9)

yic＝yiα+yi(10)

zic＝ziα+zi(11)

其中：(xiα,yiα,ziα)是髋关节在机器人机身坐标系中的坐标。

优选地，由权利要求书中公式(12)，步骤2中，设六足机器人第n次转动时机身坐标系相对世界坐标系的方向余弦矩阵为：

机身坐标系原点c在世界坐标系o中位置矢量为^opcn＝(^oxcn,^oycn,^ozcn)^t，三条支撑足末端在世界坐标系中的位置矢量分别为p1o、p2o、p3o，其中：pio＝(xio,yio,zio)^t

则：

由权利要求书中公式(13)，三条支撑足末端在机身坐标系c和世界坐标系o中的位置矢量有关系：

pio＝^orcnpic+^opcn(13)

将公式(13)展开，取第三分量上的方程，得：

其中：rni＝r31nxi0+r32nyi0+r33nzi0为已知常数项；

由公式(14)得，当第i足的髋关节每次转动角度δα、转动n次后：

z1o-^ozcn＝rn1+r′31nx1c+r′32ny1c+r33nz1c(15)

即权利要求书中公式(15)、(16)、(17)，其中：

r′31n＝r31ncosnδα-r32nsinnδα(16)

r′32n＝r31nsinnδα+r32ncosnδα(17)

在平地上，足末端位置在世界坐标系中zio相同，即z1o＝z2o＝z3o，由权利要求书中公式(18)及(19)得：

r′31nx1+r′32ny1+r33nz1-r31nx2-r32ny2-r33nz2＝rn2-rn1(18)

r′31nx1+r′32ny1+r33nz1-r31nx3-r32ny3-r33nz3＝rn3-rn1(19)

因此，得到关于p＝(x1,y1...y3,z3)^t的非齐次线性方程组，对支撑足关节的任意一次转动均得到两个方程。

优选地，步骤4及步骤5中转动5次得到五组imu传感器中姿态不同的三自由度角度数据，取n＝0,1,2...4，得10个方程，取其中9个，解出p＝(x1,y1...y3,z3)^t；取其中第一至第九式，得权利要求书中公式(20)：

根据克拉默法则可求解支撑足足底相对机身坐标系中的坐标。

优选地，步骤6中，对五组不相同的imu传感器中陀螺三自由度角度数据带入方程组，通过克拉默法则求解初始各支撑足足底在机身坐标系中坐标，即反解出各支撑足的关节角度；

所述求解过程基于六足移动机器人运动学逆解，根据支撑足足底相对机身坐标系的坐标求解六足机器人关节角度：

由

xic＝xiα+[l1+l2cosβi+l3cos(βi+γi)]sinαi(21)

yic＝yiα+[l1+l2cosβi+l3cos(βi+γi)]cosαi(22)

zic＝ziα+l2sinβi+l3sin(βi+γi)(23)

则权利要求书中公式(1)、(2)及(3)得：

其中，由权利要求书中公式(4)及(5)：

a＝(xic-xiα-l1sinαi)²+(yic-yiα-l1cosαi)²(4)

b＝(zic-zi0)²(5)

更优选地，公式(4)、(5)有两个解，需要根据六足机器人的运动空间及关节的运动范围限制，选择合适的关节角度。

本发明所述标定方法针对的六足机器人，其六个结构相同的支撑足绕正六边形机身呈旋转对称布置。

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

本发明通过微型惯性传感器获取六足机器人机身姿态角度，结合足式机器人系统运动学与逆运动学模型完成多足运动机器人支撑足各关节角度校正；能快速准确完成对多足移动机器人关节角度的校正，保证机器人多足协调与运动轨迹精度。

本发明标定基于六足机器人配置的imu传感器，其具有较高的测量精度，而且标定过程只受传感器精度及测量过程数值计算的精度所影响，相比利用外置的机械标定方法具有更高的精度。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为本发明一实施例的方法流程图；

图2为本发明一实施例的集成惯性传感器的六足机器人构型与坐标系示意图；

图3为本发明一实施例的足末端在机身坐标系c和世界坐标系o中的位置矢量示意图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进。这些都属于本发明的保护范围。

如图1所示，一种基于机身姿态角度校正的六足机器人关节角度标定方法，所述方法针对的六足机器人，其六个结构相同的支撑足绕正六边形机身呈旋转对称布置。如图2所示，从机身到足底上的三个关节依次定义为：髋关节、臀关节、膝关节；在六足机器人的机身重心处设立机身坐标系，该机身坐标系中：xoy平面与机身平面平行、z轴垂直于机身平面；六足髋关节转动轴垂直于机身平面，每个支撑足上的臀关节转动轴与膝关节转动轴相互平行，且均与机身平面平行；

所述方法具体步骤如下：

步骤1、初始化imu传感器的三自由度陀螺角度

由于六足机器人支撑至少需要三条支撑足，考虑机器人内的三条足机构，并设足底到机身重心位置矢量分别为p1c,p2c,p3c，简化为：pic＝(xic,yic,zic)^t(i＝1,2,3)。如图2所示

由权利要求书中公式(6)、(7)、(8)：

xi＝(l1+l2cosβi+l3cosθi)sinαi(6)

yi＝(l1+l2cosβi+l3cosθi)cosαi(7)

zi＝l2sinβi+l3sinθi(8)

其中；αi表示第i个髋关节相对与零点的转动角度，βi表示第i个臀关节相对于零点的转动角度，γi表示第i个膝关节相对于零点的转动角度，l1为髋关节相对于机身重心距离，l2为大腿机构长度，l3为小腿结构长度。

由权利要求书中公式(9)、(10)、(11)得：

xic＝xiα+xi(9)

yic＝yiα+yi(10)

zic＝ziα+zi(11)

其中：(xiα,yiα,ziα)是髋关节在机器人机身坐标系中的坐标。

步骤2、使某一支撑足髋关节旋转一定给定角度δα，读取陀螺三自由度角度；

本实施例中，具体方法为：驱动六足移动机器人仅以任意三支撑足支撑地面，选取其中第一条支撑足并驱动该支撑足足上某一关节发生角度转动，且该支撑足足底滑动，另外第二和第三条支撑足足底与地形不滑动，保持第二和第三条支撑足固定不动，第一条支撑足转动髋关节，通过建立关节角度与姿态间的函数关系，获得关于关节零点的线性方程组，求解出支撑足各关节角度。

其原理推导如下：

设机器人第n次转动时机身坐标系相对世界坐标系的方向余弦矩阵为，由权利要求书中公式(12)：

机身坐标系原点c在世界坐标系o中位置矢量为^opcn＝(^oxcn,^oycn,^ozcn)^t，三支撑足足末端在世界坐标系中的位置矢量分别为p1o、p2o、p3o，其中：pio＝(xio,yio,zio)^t，

则：

足末端在机身坐标系c和世界坐标系o中的位置矢量(如图3所示)有关系：

pio＝^orcnpic+^opcn(13)

即权利要求书中公式(13)，将式(13)展开，取第三分量上的方程，得：

其中：rni＝r31nxi0+r32nyi0+r33nzi0为已知常数项。

由公式(14)得，当第i足的髋关节每次转动角度δα、转动n次后得到权利要求书中公式(15)：

z1o-^ozcn＝rn1+r′31nx1c+r′32ny1c+r33nz1c(15)

其中：

r′31n＝r31ncosnδα-r32nsinnδα(16)

r′32n＝r31nsinnδα+r32ncosnδα(17)

即权利要求书中公式(16)及(17)，在平地上，足末端位置在世界坐标系中zio相同，即z1o＝z2o＝z3o，得到权利要求书中公式(18)、(19)：

r′31nx1+r′32ny1+r33nz1-r31nx2-r32ny2-r33nz2＝rn2-rn1(18)

r′31nx1+r′32ny1+r33nz1-r31nx3-r32ny3-r33nz3＝rn3-rn1(19)

因此，得到关于p＝(x1,y1...y3,z3)^t的非齐次线性方程组，对支撑足关节的任意一次转动都可以得到两个方程。

步骤3、重复步骤2，直至得到五组数据；

步骤4、若步骤3得到的五组数据姿态不全相同，通过步骤5进行求解，反之则转动任意臀关节或膝关节，重复步骤2、3，直至得到五组不全相同的姿态数据；

转动5次，取n＝0,1,2...4得10个方程，只须取其中9个，解出p＝(x1,y1...y3,z3)^t。取其中第一至第九式，得权利要求书中公式(20)：

根据克拉默法则，可求解各支撑足足底相对机身坐标系中的坐标。

步骤5、根据支撑足足底相对机身坐标系的坐标，求解六足机器人某一支撑足关节角度。

其求解过程基于六足移动机器人运动学逆解。根据足底相对机身坐标系的坐标，求解六足机器人关节角度：

由公式(21)、(22)及(23)：

xic＝xiα+[l1+l2cosβi+l3cos(βi+γi)]sinαi(21)

yic＝yiα+[l1+l2cosβi+l3cos(βi+γi)]cosαi(22)

zic＝ziα+l2sinβi+l3sin(βi+γi)(23)

则由公式(1)、(2)及(3)：

其中：

a＝(xic-xiα-l1sinαi)²+(yic-yiα-l1cosαi)²(4)

b＝(zic-zi0)²(5)

如权利要求书中公式(4)及(5)，需要注意的是，公式(4)、(5)有两个解，需要根据六足机器人的运动空间及关节的运动范围限制，选择合适的关节角度。

步骤6、重复以上操作，即对六足机器人六个支撑足进行关节角度标定。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变形或修改，这并不影响本发明的实质内容。