CN107065558A - 基于机身姿态角度校正的六足机器人关节角度标定方法 - Google Patents
基于机身姿态角度校正的六足机器人关节角度标定方法 Download PDFInfo
- Publication number
- CN107065558A CN107065558A CN201710312625.5A CN201710312625A CN107065558A CN 107065558 A CN107065558 A CN 107065558A CN 201710312625 A CN201710312625 A CN 201710312625A CN 107065558 A CN107065558 A CN 107065558A
- Authority
- CN
- China
- Prior art keywords
- msub
- mtd
- mrow
- mtr
- joint
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
- G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
Abstract
本发明提供一种基于机身姿态角度校正的六足机器人关节角度标定方法，通过微型惯性传感器获取六足机器人机身姿态角度，结合足式机器人系统运动学与逆运动学模型完成多足运动机器人支撑足各关节角度校正；驱动六足机器人以任意三足支撑地面，另外三足抬起并始终保持不接触地面；选取其中一条支撑足驱动该足上某一关节发生角度转动，则该足足底将滑动，另外两足足底与地面不滑动；通过建立关节角度与姿态间的函数关系，获得关于关节零点的线性方程组，求解得支撑足足底相对机身坐标系中的坐标，再利用已知足末端位置即可反解出支撑足各关节角度。本方法能快速准确完成对多足移动机器人关节角度的校正，保证机器人多足协调与运动轨迹精度。
Description
技术领域
本发明涉及足式移动机器人运动情况的标定方法，属于机器人技术领域，具体地，涉及一种基于机身姿态角度校正的六足机器人关节角度标定方法。
背景技术
相较于轮式移动机器人，足式移动机器人因其具有灵活多变的运动模式，故拥有较强的越障能力，在非结构化环境中具备更好的运动功能鲁棒性。作为足式移动机器人的典型代表，六足移动机器人正逐渐成为足式移动机器人领域研究的热点。
由于六足移动机器人多属于混联机构，运动冗余关节较多，因此其运动控制的准确性及效率难以保证，运动学问题也就成为了六足移动机器人控制理论研究领域的主要热点之一。六足移动机器人的运动精度因其结构特征主要取决于足部各支链的协调与运动轨迹精度，并需要尽量减小足底受力对各关节角度的控制精度影响。因此需要准确的矫正各关节转动角度，从而提高运动轨迹控制精度，降低足底与地面的接触冲力。
经检索：
专利号为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＝r31ncos nΔα-r32nsin nΔα (16)
r′32n＝r31nsin nΔα+r32ncos nΔα (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)2+(yic-yiα-l1cosαi)2 (4)
B＝(zic-zi0)2 (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＝r31ncos nΔα-r32nsin nΔα (16)
r′32n＝r31nsin nΔα+r32ncos nΔα (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)2+(yic-yiα-l1cosαi)2 (4)
B＝(zic-zi0)2 (5)
如权利要求书中公式(4)及(5)，需要注意的是，公式(4)、(5)有两个解，需要根据六足机器人的运动空间及关节的运动范围限制，选择合适的关节角度。
步骤6、重复以上操作，即对六足机器人六个支撑足进行关节角度标定。
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变形或修改，这并不影响本发明的实质内容。
Claims (6)
1.一种基于机身姿态角度校正的六足机器人关节角度标定方法，其特征在于：
驱动六足移动机器人以任意三条支撑足支撑地面，选取其中第一条支撑足并驱动该第一条支撑足上某一关节发生角度转动，且该支撑足的足底滑动；第二条和第三条支撑足的足底与地面不滑动，保持第二条和第三条支撑足固定不动；
建立关节角度与姿态间的函数关系，获得关于关节零点的线性方程组，求解即解出第一条支撑足的各关节角度，从而实现对六足机器人的全部关节角度标定。
2.根据权利要求1所述的一种基于机身姿态角度校正的六足机器人关节角度标定方法，其特征在于，所述的六足机器人，其六个结构相同的支撑足绕正六边形机身呈旋转对称布置；从六足机器人机身到支撑足足底上的三个关节依次定义为：髋关节、臀关节、膝关节；
在六足机器人机身的重心处设立机身坐标系，其中：机身坐标系的xoy平面与机身平面平行，机身坐标系的z轴垂直于机身平面；六足髋关节的转动轴垂直于机身平面，每个支撑足上的臀关节转动轴与膝关节转动轴相互平行，且均与机身平面平行。
3.根据权利要求1或2所述的基于机身姿态角度校正的六足机器人关节角度标定方法，其特征在于：所述方法包括如下步骤：
步骤1、初始化六足机器人机身的IMU传感器的三自由度陀螺角度；
步骤2、使六足机器人某一支撑足任意关节旋转给定角度；
步骤3、读取IMU传感器中陀螺三自由度角度；
步骤4、使步骤2中所述关节再次旋转，再重复步骤3，直至得到五组姿态IMU传感器中陀螺三自由度角度数据；
步骤5、若步骤4得到的五组数据不全相同，则转到步骤6进行求解；
若步骤4得到的五组姿态数据有两组以上是相同的，则转动步骤2中该支撑足的任意一个关节一定角度，但该关节不得是步骤2中已旋转过的关节，重复步骤2～4，直至得到五组不全相同的姿态IMU传感器中陀螺三自由度角度姿态数据；
步骤6、根据支撑足足底相对机身坐标系的坐标，求解六足机器人步骤2中进行旋转运动的支撑足的关节角度；
步骤7、重复以上操作，即对六足机器人六个支撑足进行关节角度标定。
4.根据权利要求3所述的一种基于机身姿态角度校正的六足机器人关节角度标定方法，其特征在于，步骤1中，利用IMU传感器的三自由度陀螺感知六足机器人机身姿态角度；初始化IMU传感器，是通过IMU传感器内部的地磁偏角传感器进行校正。
5.根据权利要求3所述的一种基于机身姿态角度校正的六足机器人关节角度标定方法，其特征在于，步骤6中，求解关节角度需要建立关节零点、关节变化量同机器人位姿之间的映射关系：
六足机器人支撑至少需要三条支撑足，设支撑足足底到机身重心位置矢量分别为p1c、p2c、p3c，简化为：pic＝(xic,yic,zic)T，i＝1,2,3，令：
(xiα,yiα,ziα)为髋关节在机器人机身坐标系中的坐标；根据足底相对机身坐标系的坐标，求解六足机器人关节角：
<mrow>
<msub>
<mi>&alpha;</mi>
<mi>i</mi>
</msub>
<mo>=</mo>
<mi>arctan</mi>
<mfrac>
<mrow>
<msub>
<mi>x</mi>
<mrow>
<mi>i</mi>
<mi>c</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>x</mi>
<mrow>
<mi>i</mi>
<mi>&alpha;</mi>
</mrow>
</msub>
</mrow>
<mrow>
<msub>
<mi>y</mi>
<mrow>
<mi>i</mi>
<mi>c</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>y</mi>
<mrow>
<mi>i</mi>
<mi>&alpha;</mi>
</mrow>
</msub>
</mrow>
</mfrac>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
1
<mrow>
<msub>
<mi>&beta;</mi>
<mi>i</mi>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<mi>arccos</mi>
<mo>&lsqb;</mo>
<mi>A</mi>
<mo>+</mo>
<mi>B</mi>
<mo>-</mo>
<msubsup>
<mi>l</mi>
<mn>2</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>l</mi>
<mn>3</mn>
<mn>2</mn>
</msubsup>
<mo>&rsqb;</mo>
</mrow>
<mrow>
<mn>2</mn>
<msub>
<mi>l</mi>
<mn>2</mn>
</msub>
<msqrt>
<mrow>
<mi>A</mi>
<mo>+</mo>
<mi>B</mi>
</mrow>
</msqrt>
</mrow>
</mfrac>
<mo>+</mo>
<mfrac>
<mrow>
<mi>arctan</mi>
<msqrt>
<mi>B</mi>
</msqrt>
</mrow>
<msqrt>
<mi>A</mi>
</msqrt>
</mfrac>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>2</mn>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<msub>
<mi>&gamma;</mi>
<mi>i</mi>
</msub>
<mo>=</mo>
<mi>arccos</mi>
<mfrac>
<mrow>
<mi>A</mi>
<mo>+</mo>
<mi>B</mi>
<mo>-</mo>
<msubsup>
<mi>l</mi>
<mn>2</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>l</mi>
<mn>3</mn>
<mn>2</mn>
</msubsup>
</mrow>
<mrow>
<mn>2</mn>
<msub>
<mi>l</mi>
<mn>2</mn>
</msub>
<msub>
<mi>l</mi>
<mn>3</mn>
</msub>
</mrow>
</mfrac>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>3</mn>
<mo>)</mo>
</mrow>
</mrow>
其中：
A＝(xic-xiα-l1 sinαi)2+(yic-yiα-l1 cosαi)2 (4)
B＝(zic-zi0)2 (5)
αi表示第i个髋关节相对零点的转动角度，βi表示第i个臀关节相对零点的转动角度，γi表示第i个膝关节相对零点的转动角度，髋关节相对于机身重心距离为l1，大腿机构长度为l2，小腿结构长度为l3。
6.根据权利要求3所述的一种基于机身姿态角度校正的六足机器人关节角度标定方法，其特征在于，步骤4中，为了保证求解的有效性，任意一条支撑足的某一关节转动5次并记录相对转角，且5次获得的机身姿态角度不全相同；
令
xi＝(l1+l2 cosβi+l3 cosθi)sinαi (6)
yi＝(l1+l2 cosβi+l3 cosθi)cosαi (7)
zi＝l2 sinβi+l3 sinθi (8)
则：
xic＝xiα+xi (9)
yic＝yiα+yi (10)
zic＝ziα+zi (11)
设机器人第N次转动时机身坐标系相对世界坐标系的方向余弦矩阵为：
<mrow>
<msub>
<mmultiscripts>
<mi>R</mi>
<mi>o</mi>
</mmultiscripts>
<mrow>
<mi>c</mi>
<mi>n</mi>
</mrow>
</msub>
<mo>=</mo>
<mfenced open = "(" close = ")">
<mtable>
<mtr>
<mtd>
<msub>
<mi>r</mi>
<mrow>
<mn>11</mn>
<mi>n</mi>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>r</mi>
<mrow>
<mn>12</mn>
<mi>n</mi>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>r</mi>
<mrow>
<mn>13</mn>
<mi>n</mi>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>r</mi>
<mrow>
<mn>21</mn>
<mi>n</mi>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>r</mi>
<mrow>
<mn>22</mn>
<mi>n</mi>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>r</mi>
<mrow>
<mn>23</mn>
<mi>n</mi>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>r</mi>
<mrow>
<mn>31</mn>
<mi>n</mi>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>r</mi>
<mrow>
<mn>32</mn>
<mi>n</mi>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>r</mi>
<mrow>
<mn>33</mn>
<mi>n</mi>
</mrow>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>12</mn>
<mo>)</mo>
</mrow>
</mrow>
机身坐标系原点C在世界坐标系O中位置矢量为opcn＝(oxcn,oycn,ozcn)T，三支撑足末端在世界坐标系中的位置矢量分别为p1o、p2o、p3o，其中：pio＝(xio,yio,zio)T，支撑足末端在机身坐标系C和世界坐标系O中的位置矢量有关系：
pio＝oRcnpic+opcn (13)
取第三分量上的方程，得：
<mrow>
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>z</mi>
<mrow>
<mi>i</mi>
<mi>o</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mmultiscripts>
<mi>z</mi>
<mi>o</mi>
</mmultiscripts>
<mrow>
<mi>c</mi>
<mi>n</mi>
</mrow>
</msub>
<mo>=</mo>
<msub>
<mi>r</mi>
<mrow>
<mn>31</mn>
<mi>n</mi>
</mrow>
</msub>
<msub>
<mi>x</mi>
<mrow>
<mi>i</mi>
<mi>c</mi>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>r</mi>
<mrow>
<mn>32</mn>
<mi>n</mi>
</mrow>
</msub>
<msub>
<mi>y</mi>
<mrow>
<mi>i</mi>
<mi>c</mi>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>r</mi>
<mrow>
<mn>33</mn>
<mi>n</mi>
</mrow>
</msub>
<msub>
<mi>z</mi>
<mrow>
<mi>i</mi>
<mi>c</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>=</mo>
<msub>
<mi>r</mi>
<mrow>
<mi>n</mi>
<mi>i</mi>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>r</mi>
<mrow>
<mn>31</mn>
<mi>n</mi>
</mrow>
</msub>
<msub>
<mi>x</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<msub>
<mi>r</mi>
<mrow>
<mn>32</mn>
<mi>n</mi>
</mrow>
</msub>
<msub>
<mi>y</mi>
<mi>i</mi>
</msub>
<mo>+</mo>
<msub>
<mi>r</mi>
<mrow>
<mn>33</mn>
<mi>n</mi>
</mrow>
</msub>
<msub>
<mi>z</mi>
<mi>i</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>14</mn>
<mo>)</mo>
</mrow>
</mrow>
其中，rni＝r31nxi0+r32nyi0+r33nzi0为已知常数项；
当第i足的髋关节每次转动角度Δα，转动n次后，n＝0,1,2...4：
z1o-ozcn＝rn1+r′31nx1c+r′32ny1c+r33nz1c (15)
其中：
r′31n＝r31n cosnΔα-r32n sinnΔα (16)
r′32n＝r31n sin nΔα+r32n cos nΔα (17)
在平地上，足末端位置在世界坐标系中zio相同，即z1o＝z2o＝z3o：
r′31nx1+r′32ny1+r33nz1-r31nx2-r32ny2-r33nz2＝rn2-rn1 (18)
r′31nx1+r′32ny1+r33nz1-r31nx3-r32ny3-r33nz3＝rn3-rn1 (19)
转动5次，取n＝0,1,2...4得10个方程，只须取其中9个，得：
<mrow>
<mfenced open = "(" close = ")">
<mtable>
<mtr>
<mtd>
<msubsup>
<mi>r</mi>
<mn>310</mn>
<mo>&prime;</mo>
</msubsup>
</mtd>
<mtd>
<msubsup>
<mi>r</mi>
<mn>320</mn>
<mo>&prime;</mo>
</msubsup>
</mtd>
<mtd>
<msub>
<mi>r</mi>
<mn>330</mn>
</msub>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>r</mi>
<mn>310</mn>
</msub>
</mrow>
</mtd>
<mtd>
<mo>...</mo>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>r</mi>
<mn>310</mn>
<mo>&prime;</mo>
</msubsup>
</mtd>
<mtd>
<msubsup>
<mi>r</mi>
<mn>320</mn>
<mo>&prime;</mo>
</msubsup>
</mtd>
<mtd>
<msub>
<mi>r</mi>
<mn>330</mn>
</msub>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mo>...</mo>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>r</mi>
<mn>330</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>r</mi>
<mn>311</mn>
<mo>&prime;</mo>
</msubsup>
</mtd>
<mtd>
<msubsup>
<mi>r</mi>
<mn>321</mn>
<mo>&prime;</mo>
</msubsup>
</mtd>
<mtd>
<msub>
<mi>r</mi>
<mn>331</mn>
</msub>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>r</mi>
<mn>311</mn>
</msub>
</mrow>
</mtd>
<mtd>
<mo>...</mo>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>r</mi>
<mn>311</mn>
<mo>&prime;</mo>
</msubsup>
</mtd>
<mtd>
<msubsup>
<mi>r</mi>
<mn>321</mn>
<mo>&prime;</mo>
</msubsup>
</mtd>
<mtd>
<msub>
<mi>r</mi>
<mn>331</mn>
</msub>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mo>...</mo>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>r</mi>
<mn>331</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>...</mo>
</mtd>
<mtd>
<mo>...</mo>
</mtd>
<mtd>
<mo>...</mo>
</mtd>
<mtd>
<mo>...</mo>
</mtd>
<mtd>
<mo>...</mo>
</mtd>
<mtd>
<mo>...</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>r</mi>
<mn>315</mn>
<mo>&prime;</mo>
</msubsup>
</mtd>
<mtd>
<msubsup>
<mi>r</mi>
<mn>325</mn>
<mo>&prime;</mo>
</msubsup>
</mtd>
<mtd>
<msub>
<mi>r</mi>
<mn>335</mn>
</msub>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>r</mi>
<mn>315</mn>
</msub>
</mrow>
</mtd>
<mtd>
<mo>...</mo>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "(" close = ")">
<mtable>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mn>1</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>y</mi>
<mn>1</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>z</mi>
<mn>1</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mn>2</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>...</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>z</mi>
<mn>3</mn>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "(" close = ")">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>r</mi>
<mn>02</mn>
</msub>
<mo>-</mo>
<msub>
<mi>r</mi>
<mn>01</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>r</mi>
<mn>03</mn>
</msub>
<mo>-</mo>
<msub>
<mi>r</mi>
<mn>01</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>r</mi>
<mn>12</mn>
</msub>
<mo>-</mo>
<msub>
<mi>r</mi>
<mn>11</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>r</mi>
<mn>13</mn>
</msub>
<mo>-</mo>
<msub>
<mi>r</mi>
<mn>11</mn>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>...</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>r</mi>
<mn>52</mn>
</msub>
<mo>-</mo>
<msub>
<mi>r</mi>
<mn>51</mn>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>20</mn>
<mo>)</mo>
</mrow>
</mrow>
根据克拉默法则，求解支撑足足底相对机身坐标系中的坐标，再利用已知足末端位置即解出支撑足各关节角度，以此进行角度标定。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710312625.5A CN107065558B (zh) | 2017-05-05 | 2017-05-05 | 基于机身姿态角度校正的六足机器人关节角度标定方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710312625.5A CN107065558B (zh) | 2017-05-05 | 2017-05-05 | 基于机身姿态角度校正的六足机器人关节角度标定方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107065558A true CN107065558A (zh) | 2017-08-18 |
| CN107065558B CN107065558B (zh) | 2019-09-27 |
Family
ID=59597290
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710312625.5A Expired - Fee Related CN107065558B (zh) | 2017-05-05 | 2017-05-05 | 基于机身姿态角度校正的六足机器人关节角度标定方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107065558B (zh) |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110032207A (zh) * | 2019-05-08 | 2019-07-19 | 吉林大学 | 机身等效连杆模型建立方法及多足协调控制方法 |
| CN111290272A (zh) * | 2020-02-15 | 2020-06-16 | 中国海洋大学 | 基于多足机器人的姿态平稳性调整方法 |
| CN112243401A (zh) * | 2018-06-05 | 2021-01-19 | 索尼公司 | 信息处理装置、信息处理方法以及程序 |
| CN112965508A (zh) * | 2021-02-03 | 2021-06-15 | 北京理工大学 | 多信息反馈的电动并联轮足机器人行走控制方法和系统 |
| CN114153218A (zh) * | 2021-12-16 | 2022-03-08 | 广州城市理工学院 | 一种机器人腿部支撑算法 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP1034066A1 (en) * | 1997-12-01 | 2000-09-13 | Giddings & Lewis | System and method for calibrating a hexapod positioning device |
| CN103231375A (zh) * | 2013-04-28 | 2013-08-07 | 苏州大学 | 基于距离误差模型的工业机器人标定方法 |
| DE102012015056A1 (de) * | 2012-07-28 | 2014-02-13 | Bsautomatisierung Gmbh | Robotersteuerungsvorrichtung |
| CN104608129A (zh) * | 2014-11-28 | 2015-05-13 | 江南大学 | 基于平面约束的机器人标定方法 |
| CN105808882A (zh) * | 2016-03-29 | 2016-07-27 | 郑州轻工业学院 | 仿爬行类四足步行机器人运动参数的标定方法与装置 |
-
2017
- 2017-05-05 CN CN201710312625.5A patent/CN107065558B/zh not_active Expired - Fee Related
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP1034066A1 (en) * | 1997-12-01 | 2000-09-13 | Giddings & Lewis | System and method for calibrating a hexapod positioning device |
| DE102012015056A1 (de) * | 2012-07-28 | 2014-02-13 | Bsautomatisierung Gmbh | Robotersteuerungsvorrichtung |
| CN103231375A (zh) * | 2013-04-28 | 2013-08-07 | 苏州大学 | 基于距离误差模型的工业机器人标定方法 |
| CN104608129A (zh) * | 2014-11-28 | 2015-05-13 | 江南大学 | 基于平面约束的机器人标定方法 |
| CN105808882A (zh) * | 2016-03-29 | 2016-07-27 | 郑州轻工业学院 | 仿爬行类四足步行机器人运动参数的标定方法与装置 |
Non-Patent Citations (4)
| Title |
|---|
| JUNG-MIN YANG: "Tripod gaits for fault tolerance of hexapod walking machines with a locked joint failure", 《ROBOTICS AND AUTONOMOUS SYSTEMS》 * |
| 周学才 等: "机器人理论关节参数的标定方法及软件设计", 《机器人》 * |
| 庄红超 等: "电驱动重载六足机器人关节转速分析方法", 《机械工程学报》 * |
| 杨许 等: "仿爬行类四足步行机器人运动学标定模型研究", 《机械传动》 * |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112243401A (zh) * | 2018-06-05 | 2021-01-19 | 索尼公司 | 信息处理装置、信息处理方法以及程序 |
| CN112243401B (zh) * | 2018-06-05 | 2024-02-20 | 索尼公司 | 信息处理装置、信息处理方法以及程序 |
| CN110032207A (zh) * | 2019-05-08 | 2019-07-19 | 吉林大学 | 机身等效连杆模型建立方法及多足协调控制方法 |
| CN111290272A (zh) * | 2020-02-15 | 2020-06-16 | 中国海洋大学 | 基于多足机器人的姿态平稳性调整方法 |
| CN111290272B (zh) * | 2020-02-15 | 2021-02-09 | 中国海洋大学 | 基于多足机器人的姿态平稳性调整方法 |
| CN112965508A (zh) * | 2021-02-03 | 2021-06-15 | 北京理工大学 | 多信息反馈的电动并联轮足机器人行走控制方法和系统 |
| CN112965508B (zh) * | 2021-02-03 | 2022-02-15 | 北京理工大学 | 多信息反馈的电动并联轮足机器人行走控制方法和系统 |
| CN114153218A (zh) * | 2021-12-16 | 2022-03-08 | 广州城市理工学院 | 一种机器人腿部支撑算法 |
| CN114153218B (zh) * | 2021-12-16 | 2023-12-08 | 广州城市理工学院 | 一种机器人腿部支撑算法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107065558B (zh) | 2019-09-27 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107065558A (zh) | 基于机身姿态角度校正的六足机器人关节角度标定方法 | |
| CN107214698B (zh) | 基于机身重心位移校正的六足机器人关节角度标定方法 | |
| CN103991078B (zh) | 机器人系统和用于控制该机器人系统的方法 | |
| Merlet | MARIONET, a family of modular wire-driven parallel robots | |
| CN108972559A (zh) | 基于红外立体视觉定位系统和机械臂的手眼标定方法 | |
| Williams et al. | 3D Cable‐Based Cartesian Metrology System | |
| CN106323294A (zh) | 变电站巡检机器人定位方法及定位装置 | |
| CN107160380A (zh) | 一种基于scara机械手的相机标定和坐标变换的方法 | |
| CN104354166B (zh) | 一种三自由度并联机器人的零点标定方法 | |
| CN110919658B (zh) | 一种基于视觉和多坐标系闭环转换的机器人标定方法 | |
| CN107145071B (zh) | 一种空间系绳系统的拉格朗日动力模型及控制器 | |
| CN105992900A (zh) | 用于计算装置的取向的系统和方法 | |
| CN103869834B (zh) | 基于经验模态法的三轴气浮台质心智能调节方法 | |
| CN206321253U (zh) | 变电站巡检机器人定位装置 | |
| CN108736657B (zh) | 一种基于光学传感器的永磁球形电机转子位置检测方法 | |
| Mihelj et al. | Robotics | |
| Rodriguez Leal et al. | From origami to a new class of centralized 3-DOF parallel mechanisms | |
| CN114216456B (zh) | 一种基于imu与机器人本体参数融合的姿态测量方法 | |
| CN109262659B (zh) | 一种机械臂关节传感器的零位校准方法和设备 | |
| CN106625774A (zh) | 一种空间机械臂几何参数标定方法 | |
| CN106239510A (zh) | 一种三自由度并联机器人零点标定方法 | |
| CN105808882A (zh) | 仿爬行类四足步行机器人运动参数的标定方法与装置 | |
| CN103868648A (zh) | 三轴气浮仿真实验平台的质心测量方法 | |
| CN114842090A (zh) | 基于精密角度基准的视觉惯性标定系统及其标定方法 | |
| CN110986934B (zh) | 一体化双轴旋转惯导天文组合导航系统的导航方法及系统 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| CF01 | Termination of patent right due to non-payment of annual fee | ||
| CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20190927 |