CN105808882A - 仿爬行类四足步行机器人运动参数的标定方法与装置 - Google Patents

Abstract

本发明公开了一种仿爬行类四足步行机器人运动参数的标定装置，包括底基平台，底基平台上设有安装仿爬行类四足步行机器人腿主体的链接球铰，链接球铰通过定位销和螺栓固定在底基平台上，链接球铰和底基平台之间设有记录纸，建立专用的标定装置，结构简单；在此基础上，还公开了一种仿爬行类四足步行机器人运动参数的标定方法，将仿爬行类四足步行机器人的四条腿主体安装在测量平台上，采用简单的测量装置，测量机器人的运动轨迹，通过在矩形机体和测量平台上建立不同的坐标系计算其运动前后一系列的运动参数，就完成对该类机器人的几何参数标定。

Description

仿爬行类四足步行机器人运动参数的标定方法与装置

技术领域

本发明涉及机器人的技术领域，具体涉及一种仿爬行类四足步行机器人运动参数的标定方法与装置。

背景技术

实际应用中，步行机器人需要根据基础坐标系、机体坐标系及目标之间的空间几何关系驱动机器人的关节伺服电机动作，完成相关作业任务。然而，多足步行机器人关节之间的几何参数的误差将会使其质心或者安装在其上的机械臂末端操作器的位姿偏离实际要求。

由于步行机器人的机械加工误差、安装误差、控制系统误差、传动误差、构件变形等已成为步行机器人机身平台位姿误差的主要原因，所以通过测量步行机器人机身的位姿误差，并找出相对准确的结构参数，再通过控制算法补偿步行机器人的伺服电机的控制系统，对控制模型修正后驱动步行机器人运动，可以在降低成本的同时达到更高的精度。因此，对步行机器人开展参数标定是十分重要的。这类问题通常利用以下4个步骤解决：

(1)误差分析，找出可能存在的系统误差参数,建立误差参数与可测量参数之间的数学模型；

(2)通过各种检测手段得到这些可测量参数的实际测量值；

(3)把实际测量值代入数学模型，应用系统辨识等方法得到系统的误差参数；

(4)根据辨识出的误差参数，对电机的控制模型进行修正，实施控制补偿，提高机器人的运动精度。

为了开展步行机器人运动参数标定，必须研究对其机体位姿检测的问题。对于该问题，利用6维检测仪器测量6维位姿是较直接的办法。但该方法所用的设备昂贵，很难在实际工程中应用。也有学者研究采用视觉或图像与超声传感器融合进行机器人位姿的测量，但该方法的检测精度与目标的活动范围成反比，在一个较大的范围内就无法保持相应的精度，而且处理数据的过程较为麻烦。有学者研究采用专用机构坐标测量机或者采用附加测量装置接在动静平台之间，通过对机构的运动学分析，求解上平台位姿。采用这种方式时，测量装置自身精度对整体测量精度有很大的影响。

为了在低成本条件下达到一定的测量精度，一些学者利用部分测量信息间接测量机器人的末端位姿。比较有代表性的测量6维位姿的仪器包含球杆仪、拉线式传感器以及超声波传感器。这些方法的基本原理是通过测量上下平台一定数量点之间的直线距离来计算上平台的6维位姿。但该方法能够检测的工作空间范围较小，且通常需要同时使用多个仪器。也有学者提出通过间接测量机器人末端位姿，利用单方向坐标增量来得到机器人的末端位姿。但总体看，对仿爬行类多足步行机器人机体的位姿开展测量，还很少开展研究。

发明内容

为了采用低成本实施对仿爬行类多足步行机器人的参数标定，本发明提供了一种仿爬行类四足步行机器人运动参数的标定方法与装置，结构简单，可以测量仿爬行类四足步行机器人的运动轨迹，通过一系列的运动参数计算，就可以完成对该类机器人的几何参数标定。

为了达到上述目的，本发明的技术方案是：一种仿爬行类四足步行机器人运动参数的标定装置，包括底基平台，所述底基平台上设有安装仿爬行类四足步行机器人的腿主体的链接球铰，链接球铰通过定位销和螺栓固定在底基平台上，链接球铰和底基平台之间设有记录纸。

所述仿爬行类四足步行机器人包括四条腿主体和矩形机体，腿主体包括髋关节连接杆、大腿关节连接杆和小腿关节连接杆，矩形机体与髋关节连接杆相连接，髋关节连接杆与大腿关节连接杆相连接，大腿关节连接杆与小腿关节连接杆相连接。

所述链接球铰包括球销座、球销上盖和中空球芯，球销上盖通过球销螺栓将中空球芯活动设置在球销座上，球销座通过螺栓固定在底基平台上。

一种仿爬行类四足步行机器人运动参数的标定方法，其步骤如下：

步骤一：对仿爬行类四足步行机器人建立几何参数模型：设仿爬行类四足步行机器人的初始状态为矩形机体与腿主体立足点A_I、A_J、A_K、A_L所在的平面平行，髋关节的回转中心线垂直于机体平面，大腿关节的回转中心线与小腿关节的回转中心线平行于机体平面，髋关节连接杆、大腿关节连接杆、小腿关节连接杆的结构对称面重合，在底基平台的投影在一条直线上；以仿爬行类四足步行机器人的矩形机体的中心C为原点、平行于矩形机体的边为坐标轴建立机体坐标系∑_C；设矩形机体的两边长度分别是2m和2n，E_C、F_C、G_C、D_C分别为矩形机体各边的中心，E_C、F_C、G_C、D_C在腿主体的立足点A_I、A_J、A_K、A_L所在平面的投影点分别是E_O、F_O、G_O和D_O，以F₀D₀和E₀G₀的交点O_O为原点、F₀D₀和E₀G₀所在直线为坐标轴建立基础坐标系Σ_o，则腿主体的立足点A_K,A_J,A_L,A_I在基础坐标系Σ_o中的坐标为腿主体由上到下的髋关节连接杆长为l₁、大腿关节连接杆长为l₂和小腿关节连接杆长为l₃，髋关节连接杆与大腿关节连接杆之间的夹角为大腿关节连接杆与小腿关节连接杆之间的夹角为χ_i，髋关节的旋转角φ_i，其中i＝I、J、K、L；

步骤二：在基础坐标系Σ_o上，以E₀、F₀、G₀和D₀为矩形机体在Σ_o中的对应投影矩形的各边的中点，作出投影矩形(分别过G₀、E₀作平行于F₀D₀的直线，过F₀、D₀作平行于G₀E₀的直线，四线相交形成投影矩形)。投影矩形的各顶点分别取为B_I、B_J、B_K、B_L；在投影矩形的各顶点B_I、B_J、B_K、B_L处分别连接相应的安装腿的链接球铰的中心位置A_I、A_J、A_K、A_L，再分别测量A_IB_I、A_JB_J、A_KB_K、A_LB_L的长度S_I、S_J、S_K、S_L，A_IB_I、A_JB_J、A_KB_K、A_LB_L与矩形边的角度φ_i，确定各腿主体的投影长度及投影相对与矩形机体的角度；

步骤三：测量矩形机体各边的中心E_C、F_C、G_C和D_C到基础坐标系Σ_o的距离分别为h_E、h_F、h_G和h_D；

步骤四：通过控制系统驱动仿爬行类四足步行机器人运动不同步态，形成不同的运动姿态i₀。在此采用机器人机体的平动姿态为测量目标。即运动中，机器人的机体只作与初始姿态平行的运动。记录机器人的矩形机体各边的中心点E_C、F_C、G_C和D_C在基础坐标系Σ_o上的投影点和(文中的i₀＝1、2、3、4….n,表示不同的位置的变化，下同)；矩形机体各边的中心点E_C、F_C、G_C和D_C到基础坐标系Σ_o的距离分别为和测量各步态下矩形机体的中心投影在基础坐标系Σ_o中的坐标和矩形机体绕基础坐标系Σ_o的z轴的偏转角偏差

步骤五：通过测量数据计算仿爬行类四足步行机器人矩形机体绕基础坐标系∑_C的y轴运动的回转角偏差和绕x轴运动的俯仰角偏差并利用RPY模型描述爬行类四足步行机器人的方向矩阵R_c；

步骤六：利用偏转角回转角偏差俯仰角偏差确定方向矩阵R_c的误差矩阵

步骤七：利用矩形机体的轨迹(^ox_c,^oy_c,^oz_c)及误差(Δ^ox_c,Δ^oy_c,Δ^oz_c)、矩形机体尺寸(m，n)及误差(Δm,Δn)、各腿主体立足点的轨迹及误差各腿关节的长度(l_1i，l_2i，l_3i)及误差(△l_1i，△l_2i，△l_3i)、各腿关节角度值及误差误差矩阵ΔR_c的关系建立参数方程；用参数方程求解腿主体关节的误差△l_1i、△l_2i、△l_3i和矩形机体尺寸的误差Δm、Δn；

步骤八：将腿关节的误差△l_1i、△l_2i、△l_3i和矩形机体尺寸误差Δm、Δn代入校验方程确定标定的结果是否满足精度要求。

所述计算矩形机体绕基础坐标系Σ_o的z轴的偏转角偏差的方法是：以矩形机体各边的中点E_C、F_C、G_C和D_C在基础坐标系Σ_o上的投影点 和为各边的中点，组成投影矩形在矩形机体的投影矩形图上，采用将初始的图形E₀F₀G₀D₀平移坐标获得新的图形则图形与图形中心线的夹角为偏转角偏差

所述计算回转角偏差和俯仰角偏差的方法是：根据矩形机体各边的中心点E_C、F_C、G_C和D_C到基础坐标系Σ_o的距离与矩形机体的尺寸关系可知：计算回转角偏差和俯仰角偏差

本发明的有益效果：建立专用的标定装置，将仿爬行类四足步行机器人的四条腿主体安装在测量平台上，采用简单的测量装置，测量机器人的运动轨迹，通过在矩形机体和测量平台上建立不同的坐标系计算其运动前后一系列的运动参数，完成对该类机器人的几何参数标定。

附图说明

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

图1为本发明仿爬行类四足步行机器人的结构示意图。

图2为本发明标定装置的结构示意图。

图3为本发明标定装置中球销的结构示意图。

图4为本发明仿爬行类四足步行机器人安装在标定装置上的结构示意图。

图5为本发明对仿爬行类四足步行机器人建立几何参数模型。

图6为本发明矩形机体所在平面在基础坐标系上的投影。

图7为本发明矩形机体位置变化及偏转关系的结构示意图。

图8为本发明的仿爬行类四足步行机器人腿结构运动参数示意图。

具体实施方式

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

如图1所示，仿爬行类四足步行机器人包括四条腿主体7和矩形机体5，矩形机体5设置在腿主体7的中部。腿主体7包括髋关节连接杆7-2、大腿关节连接杆7-4和小腿关节连接杆7-6，矩形机体5通过髋关节7-1与髋关节连接杆7-2相连接，髋关节连接杆通过大腿关节7-3与大腿关节连接杆7-4相连接，大腿关节连接杆7-4通过小腿关节7-5与小腿关节连接杆7-6相连接。腿主体7通过髋关节7-1与矩形机体5相连接，髋关节7-1的轴线与矩形机体5垂直。大腿关节7-3的回转中心线与小腿关节7-5的回转中心线平行于矩形机体平面。通过控制髋关节连接杆7-2、大腿关节连接杆7-4和小腿关节连接杆7-6相对于各关节回转中心线的运动，带动矩形机体5随着腿主体7的移动而移动。

如图2至图4所示，一种仿爬行类四足步行机器人运动参数的标定装置，包括底基平台1，所述底基平台1上设有安装仿爬行类四足步行机器人的腿主体7的链接球铰2，底基平台1上预先加工若干个链接球铰2的安装孔位。链接球铰2的位置根据仿爬行类四足步行机器人的测试姿态的要求固定在底基平台1。链接球铰2设有四个，腿主体7的小腿关节连接杆7-6的底部安装在链接球铰2上。链接球铰2通过定位销3和螺栓4固定在底基平台1上，链接球铰2和底基平台1之间设有记录纸6，通过在纸上作标记，以记录机器人移动时，不同姿态下的矩形机体的投影轨迹。

链接球铰2包括球销座2-1、球销上盖2-2和中空球芯2-4，球销上盖2-2通过球销螺栓2-3将中空球芯2-4活动设置在球销座2-1上，球销座2-1通过螺栓固定在底基平台1上。当仿爬行类四足步行机器人固定在链接球铰2上后，可以通过控制系统驱动其运动，形成不同的运动姿态，记录纸6记录仿爬行类四足步行机器人在运动前后的运动轨迹参数。

一种仿爬行类四足步行机器人运动参数的标定方法，其步骤如下：

步骤一：对仿爬行类四足步行机器人建立几何参数模型，如图5所示。

设仿爬行类四足步行机器人的初始状态为矩形机体5与腿主体7的立足点A_I、A_J、A_K、A_L所在的平面平行，当机器人运动时，矩形机体5呈现不同的位置，在矩形机体5上取四个特殊的点(通常取矩形机体5各边的中间点)，利用吊垂线或者激光投射的方式，找到这四个点垂直投影到底基平台1的对应投影点，并在记录纸上对应标记这些点。分别连接矩形机体5上两个相对边上中间点在底基平台1的投影点，形成两条相互垂直的直线。这两条直线的交叉点为矩形机体5的中心在运动中的轨迹在底基平台1上的投影点。将机器人以初始状态固定在标定装置上。以机器人的矩形机体5的中心C为原点、平行于矩形机体5的边为坐标轴建立机体坐标系Σ_c。设矩形机体5的两边长度分别是2m和2n，E_C、F_C、G_C和D_C分别为矩形机体5各边的中心，E_C、F_C、G_C和D_C在腿主体7的立足点A_I、A_J、A_K、A_L所在平面的投影点分别是E_O、F_O、G_O和D_O，以F_OD_O连线和E_OG_O连线的交点O_O为原点、F_OD_O和E_OG_O所在直线为坐标轴建立基础坐标系Σ_o，则腿主体7的立足点A_I、A_J、A_K、A_L在基础坐标系Σ_o中的坐标为其中i＝I、J、K、L。在初始状态下，机体坐标系∑_C和基础坐标系Σ_o的原点的连线与基础坐标系Σ_o所在平面垂直，即此时机体坐标系∑_C和基础坐标系Σ_o的Z轴在同一条直线上。

腿主体7由上到下的髋关节连接杆长为l₁、大腿关节连接杆长为l₂和小腿关节连接杆长为l₃，髋关节连接杆与大腿关节连接杆之间的夹角为大腿关节连接杆l₂与小腿关节连接杆l₃之间的夹角为χ_i，其中i＝I、J、K、L。初始状态下，χ_I＝χ_J＝χ_K＝χ_L＝90°。

步骤二：在基础坐标系Σ_o中以E₀、F₀、G₀和D₀为矩形机体在Σ_o中的对应投影矩形的各边的中点，作出投影矩形，即分别过G₀、E₀作平行于F₀D₀的直线，过F₀、D₀作平行于G₀E₀的直线，四线相交形成投影矩形。投影矩形的各顶点为B_I、B_J、B_K、B_L，在矩形的各顶点B_I、B_J、B_K、B_L处分别连接相应的安装腿的链接球铰的中心位置A_I、A_J、A_K、A_L，如图6所示。再分别测量A_IB_I、A_JB_J、A_KB_K、A_LB_L的长度S_I，S_J，S_K，S_L，A_IB_I、A_JB_J、A_KB_K、A_LB_L与矩形边的角度φ_i，确定各腿主体的投影长度及投影相对与矩形机体的角度。矩形的各顶点B_I、B_J、B_K、B_L分别与安装腿的链接球铰的中心位置A_I、A_J、A_K、A_L的连线，为通过矩形的各顶点B_I、B_J、B_K、B_L的相应腿主体的中心的对称面的投影线。角度φ_i为髋关节连接杆与矩形机体的边的角度(在机器人学上，亦称为髋关节运动角)，如图6所示。初始状态下，φ_I＝45°,φ_J＝135°,φ_K＝315°,φ_L＝225°。

步骤三：测量矩形机体各边的中心E_C、F_C、G_C和D_C到基础坐标系Σ_o的距离h_A、h_B、h_C和h_D。

矩形机体各边的中心E_C、F_C、G_C和D_C到基础坐标系Σ_o的投影分别为E_O、F_O、G_O和D_O，E_C、F_C、G_C和D_C在基础坐标系Σ_o中的z轴坐标即为矩形机体各边的中心E_C、F_C、G_C和D_C到基础坐标系Σ_o的距离，分别为h_E、h_F、h_G、h_D。为了测量机器人矩形机体的每个运动位置的质心实际高度^oz_c，在实验过程中，通过测量h_E、h_F、h_G、h_D的值进行求得。在此，假设仿爬行类四足步行机器人的矩形机体是刚体，而不考虑它在结构上的扭转变形。

步骤四：通过控制系统驱动仿爬行类四足步行机器人运动，形成不同的运动姿态i₀。在此采用机器人机体的平动姿态为测量目标，即运动中机器人的机体只作与初始位置平行的运动。记录机器人的矩形机体各边的中点E_C、F_C、G_C和D_C在基础坐标系Σ_o上的投影点和矩形机体各边的中点E_C、F_C、G_C和D_C到基础坐标系Σ_o的距离测量该步态下矩形机体的中心投影在基础坐标系Σ_o中的坐标和矩形机体绕基础坐标系Σ_o的z轴的偏转角偏差

仿爬行类四足步行机器人运动时，矩形机体各边的中点E_C、F_C、G_C和D_C在基础坐标系Σ_o上的投影点分别为和以和为各边的中点，组成机器人运动后机体实际位置的投影矩形，该投影矩形的中心在基础坐标系Σ_o上的坐标即为矩形机体坐标系∑_C的原点在基础坐标系Σ_o中的投影

，如图7所示。在矩形机体的投影矩形图上，采用将初始图形E₀F₀G₀D₀平移坐标获得新的图形测量与间的夹角以及矩形机体的中心投影在基础坐标系Σ_o中的坐标获得矩形机体投影在z轴方向上的偏转角偏差

步骤五：通过测量数据计算仿爬行类四足步行机器人矩形机体的回转角偏差Δβ_s和俯仰角偏差Δγ_s，利用RPY模型描述爬行类四足步行机器人的方向矩阵R_c。

根据仿爬行类四足步行机器人矩形机体上的中点E_C、F_C、G_C和D_C到基础坐标系Σ_o的距离h_E、h_F、h_G和h_D，由回转角偏差Δβ_s和俯仰角偏差Δγ_s的定义，通过计算sinΔβ_s＝(h_F-h_D)/(2m)，sinΔγ_s＝(h_G-h_E)/(2n)，可以得出回转角偏差Δβ_s和俯仰角偏差Δγ_s。再根据偏转角偏差Δα_s、回转角偏差Δβ_s和俯仰角偏差Δγ_s，可以计算出机器人的方向矩阵R_c的误差。

为了实现对机器人方向矩阵R_c的误差测量，要建立机器人方向矩阵的误差计算模型，首先使用RPY(Roll-Pitch-Yaw)描述机器人机身姿态在运动中的变化。将矩形机体的坐标系∑_C到基础坐标系Σ_o的方向矩阵R_c表达为：

$R_c=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right].---\left(1\right)$

其中的三列分别为坐标系∑_C的X_C、Y_C、Z_C在基础坐标系Σ_o中的方向余弦。

根据机器人的运动变换理论，矩形机体的坐标系∑_C在基础坐标系Σ_o的方向矩阵R_c表达为：

$\begin{array}{c}R_C^O=R(z_o,\mathrm{\α})R(y_o,\mathrm{\β})R(x_o,\mathrm{\γ})\\ =\left[\begin{array}{ccc}c\mathrm{\α}c\mathrm{\β}& c\mathrm{\α}s\mathrm{\β}s\mathrm{\γ}-s\mathrm{\α}c\mathrm{\γ}& c\mathrm{\α}s\mathrm{\β}c\mathrm{\γ}+s\mathrm{\α}s\mathrm{\γ}\\ s\mathrm{\α}c\mathrm{\β}& s\mathrm{\α}s\mathrm{\β}s\mathrm{\γ}+c\mathrm{\α}c\mathrm{\γ}& s\mathrm{\α}s\mathrm{\β}c\mathrm{\γ}-c\mathrm{\α}s\mathrm{\γ}\\ -s\mathrm{\β}& c\mathrm{\β}s\mathrm{\γ}& c\mathrm{\β}c\mathrm{\γ}\end{array}\right]\end{array}---\left(2\right)$

式中：γ角表示(矩形机体)坐标系∑_C绕基础坐标系Σ_o的x轴转过的角度；β角表示(矩形机体)坐标系∑_C绕基础坐标系Σ_o的y轴转过的角度；α角表示(矩形机体)坐标系∑_C绕基础坐标系Σ_o的z轴转过的角度；R(z_o,α)表示齐次坐标变换时绕z轴转动的齐次变换矩阵，R(y_o,β)表示齐次坐标变换时绕y轴转动的齐次变换矩阵，R(x_o,γ)表示齐次坐标变换时绕x轴转动的齐次变换矩阵，c和s分别表示cos和sin。当运动角度γ，β，α有误差时，将引起方向矩阵R_c的变化。

步骤六：利用偏转角偏差回转角偏差俯仰角偏差确定方向矩阵R_c的误差矩阵

根据测量或者计算求得的偏转角偏差回转角偏差俯仰角偏差的值，即可求解方向矩阵的误差矩阵方向矩阵R_c的误差矩阵表示为：

${\mathrm{\ΔR}}_{{\mathrm{ci}}_0}=\left[\begin{array}{ccc}{\mathrm{\Δr}}_{11}& {\mathrm{\Δr}}_{12}& {\mathrm{\Δr}}_{13}\\ {\mathrm{\Δr}}_{21}& {\mathrm{\Δr}}_{22}& {\mathrm{\Δr}}_{23}\\ {\mathrm{\Δr}}_{31}& {\mathrm{\Δr}}_{32}& {\mathrm{\Δr}}_{33}\end{array}\right]---\left(3\right)$

而

由于标定中采用的是机器人机体的平动姿态：有

$R_c^0=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$

为由偏转角偏差回转角偏差和俯仰角偏差引起的方向矩阵的变化。将机器人运动中，实际测量的值，带入(2)式(即将(2)式中的α，β，γ用值带入)计算即可。

步骤七：由矩形机体中心的轨迹(^ox_c,^oy_c,^oz_c)及误差(Δ^ox_c,Δ^oy_c,Δ^oz_c)、矩形机体尺寸(m，n)及误差(Δm,Δn)、各腿主体立足点的轨迹及误差各腿关节的长度(l_1i，l_2i，l_3i)及误差(△l_1i，△l_2i，△l_3i)、各腿关节角度值及误差误差矩阵ΔR_c的关系建立参数方程；利用参数方程求解腿主体关节的误差△l_1i、△l_2i、△l_3i和矩形机体尺寸的误差Δm、Δn。

建立机器人矩形机体运动中方向矩阵的误差矩阵ΔR_c、矩形机体的运动轨迹(^ox_c,^oy_c,^oz_c)及误差(Δ^ox_c,Δ^oy_c,Δ^oz_c)、各腿立足点的轨迹及误差各腿关节的长度(l_1i,l_2i,l_3i)及误差(Δl_1i,Δl_2i,Δl_3i)(i＝I,J,K,L)、机器人机体尺寸(m，n)及误差(Δm,Δn)、各腿关节的角度值及误差的关系方程。在此需要注意的是，各腿关节的长度及误差可能不同，因此采用了下标i表示各腿关节的长度及误差。

各腿关节的角度值如图8所示，各尺寸之间的关系见(6)、(7)、(8)式。

式中：

由下列关系给出：

式(6)、(7)、(8)和(9)中为了描述不同位置的变化，取不同的腿主体为对象，即对各坐标值的下标i分别取i＝I,J,K,L，并引入i₀＝1,2,.....n表述不同的位置。注意到各腿关节的长度(l_1i,l_2i,l_3i)及误差(Δl_1i,Δl_2i,Δl_3i)不随位置的变化而变化，即它们没有i₀，同时注意到矩形机体尺寸的误差Δm₁和Δn₁不随坐标及位置变化。表示i腿(i＝I,J，K,L)在i₀(i₀＝1,2,3…n)位置上，各腿关节的角度值。表示A_i点在基础坐标系∑_o中的坐标值。由于设计了固定的标定平台，A_i点在基础坐标系∑_o中的坐标值不随标定位置的变化而变化，即它们没有i₀，且对应的误差取为零。为在∑_o坐标系中，机器人机体中心C在运动中的实际坐标与理论坐标的偏差。根据(参见图7，其中的值与理论值的偏差(理论值由机器人步态设计中给定)，有

为了对机器人的参数进行标定，采用不同的腿主体组合支撑地面，驱动机器人进行运动对机器人的运动参数进行测量。取一组腿主体组合运动来进行测量，可以得到11个变量及9个方程。

如：首先取I,J,L腿主体支撑地面为研究对象，此时的待求误差量为Δl_1i,Δl_2i,Δl_3i,Δm,Δn，其中i＝I,J,L。取一个运动位置i₀＝1。由式(6)、(7)、(8)产生方程9个，其中有11个变量：Δl_1i,Δl_2i,Δl_3i,Δm,Δni＝I,J,L。

再取一组腿主体支撑地面运动进行测量，可以得到3个新变量及9个方程。

如：改为用I,K,L腿主体支撑地面为研究对象，取1个位置测量i₀＝1。由式(6)、(7)、(8)产生类似式的方程9个，可以得到3个新的变量：Δl_1i,Δl_2i,Δl_3i，i＝K。

综合两次测量，共有方程18个，变量14个。可以选择其中的14个方程，将其称为参数方程，求出14个变量。将所余的4个方程记为校验方程。将校验方程写成一般的表达式

$f_{i_0}({\mathrm{\Δl}}_{1I},{\mathrm{\Δl}}_{2I},{\mathrm{\Δl}}_{3I},{\mathrm{\Δl}}_{1J},{\mathrm{\Δl}}_{2J},{\mathrm{\Δl}}_{3J},{\mathrm{\Δl}}_{1L},{\mathrm{\Δl}}_{2L},{\mathrm{\Δl}}_{3L},{\mathrm{\Δl}}_{1K},{\mathrm{\Δl}}_{2K},{\mathrm{\Δl}}_{3K},\mathrm{\Δ}m,\mathrm{\Δ}n)=0---\left(10\right)$

不失一般性，记式中i₀＝15,16,17,18。

步骤八：将腿关节的误差△l_1i、△l_2i、△l_3i和矩形机体尺寸误差Δm、Δn代入校验方程确定标定的结果是否满足精度要求。

将通过参数方程计算的结果代入校验方程。将(10)式变化为并给校验方程控制精度ε，如果：

$\left|{f_{i_0}}^{\left(1\right)}\right|\≤\mathrm{\ϵ}---\left(11\right)$

满足式(11)表明标定的结果满足精度要求，否则要重新测量。

具体的标定实例

通过设计仿爬行类四足步行机器人的运行步态，并对机器人运行过程中的实时参数进行测量记录，依据本发明的模型对机器人的参数进行标定。

以四足机器人样机MiniQuad-1为对象，其设计的理论参数为(单位m)：髋关节连杆长：l_1i＝0.065，大腿关节连杆长：l_2i＝0.200，小腿关节连杆长：l_3i＝0.266，(i＝I,J,K,L)机身尺寸：m＝0.197,n＝0.140。

设计的步态为：机器人沿基础坐标系∑₀中的y轴方向步行，要求其矩形机体与基础坐标系∑₀所在平面保持平行。设定坐标系∑_C中的y轴与基础坐标系∑₀中的y轴平行。机器人初始的位置参数为：各关节角度值分别为：

χ_I1＝90°,χ_J1＝90°,χ_K1＝90°,χ_L1＝90°、φ_I1＝45°,φ_J1＝135°,φ_K1＝315°,φ_L1＝225°

初始状态立足点A_K、A_I、A_L、A_J的位置坐标分别为：

(1)取i＝I,J,L为支撑腿主体

设计的步态为矩形机体前移0.08m，侧移-0.04m，即在初始的位置上，机器人矩形机体的质心由^op_c＝[000.266]^T运动到

利用控制系统驱动机器人重复运动3-5次，在此运动过程中立足点没有运动。而设计的

$R_c=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$

对矩形机体的运动方位矩阵的误差ΔR_c、机体的轨迹误差Δ^op_c按上述方程进行测量；对各腿关节角度及误差(其中i＝I,J,L)进行测量，可以采用安装角度编码器的方式测量获得。

给出测量值如下：矩形机体运动方位矩阵的误差矩阵ΔR_c1：

${\mathrm{\ΔR}}_{c1}=\left[\begin{array}{ccc}-0.00020& -0.00041& -0.02000\\ 0.00010& -0.00012& -0.01549\\ 0.02000& 0.01549& 0.00032\end{array}\right]$

矩形机体轨迹误差：

Δ^op_c＝[-0.00340.00080]^T

各腿立足点的误差分别为：

各关节角度的误差分别为：

Δχ_I1＝0.0213°，Δφ_I1＝0.2958°

Δχ_J1＝0.0597°，Δφ_J1＝0.0621°

Δχ_L1＝-0.1035°，Δφ_L1＝0.3270°

(2)再取I,K,L支撑腿主体为研究对象，设计的步态在初始的位置上，机身前移0.04m，侧移-0.08m。即机器人机体质心由^op_c＝[000.266]^T运动到：驱动机器人重复运动3-5次，在测量中立足点没有运动，方向矩阵R_c同(1)中所述。

与(1)中相同，测量各参数的误差为：

矩形机体运动方位矩阵的误差矩阵ΔR_c1为：

${\mathrm{\ΔR}}_{c1}=\left[\begin{array}{ccc}-0.00011& -0.00014& -0.01483\\ -0.00012& -0.00015& -0.01732\\ 0.01483& 0.01732& -0.00026\end{array}\right]$

矩形机体的轨迹误差为：

Δ^op_c＝[0.00120.00150]^T

各腿立足点的误差分别为：

各关节的角度误差分别为：

Δχ_I1＝0.0542°，Δφ_I1＝0.2967°

Δχ_K1＝0.0586°，Δφ_K1＝0°

Δχ_L1＝-0.0015°，Δφ_L1＝0.0940°。

根据方程(6)、(7)、(8)的相关模型，联立进行求解。

(3)利用MATLAB对上述联立方程求解。

(4)结果及分析

计算的腿关节的误差△l_1i、△l_2i、△l_3i和矩形机体尺寸的误差Δm、Δn如表1：

表1计算结果比较

将计算结果带入校验方程(10)有，f₁₅＝-0.003434；f₁₆＝-0.004345；f₁₇＝-0.001197；f₁₈＝0.003322。校验方程的绝对值均小于控制精度0.01，所以上述结果是合理的一组解。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

Claims (6)

1.一种仿爬行类四足步行机器人运动参数的标定装置，包括底基平台(1)，其特征在于：所述底基平台(1)上设有安装仿爬行类四足步行机器人的腿主体(7)的链接球铰(2)，链接球铰(2)通过定位销(3)和螺栓(4)固定在底基平台(1)上，链接球铰(2)和底基平台(1)之间设有记录纸(6)。

2.根据权利要求1所述的仿爬行类四足步行机器人运动参数的标定装置，其特征在于，所述仿爬行类四足步行机器人包括四条腿主体(7)和矩形机体(5)，腿主体(7)包括髋关节连接杆(7-2)、大腿关节连接杆(7-4)和小腿关节连接杆(7-6)，矩形机体(5)与髋关节连接杆(7-2)相连接，髋关节连接杆(7-2)与大腿关节连接杆(7-4)相连接，大腿关节连接杆(7-4)与小腿关节连接杆(7-6)相连接。

3.根据权利要求1或2所述的仿爬行类四足步行机器人运动参数的标定装置，其特征在于，所述链接球铰(2)包括球销座(2-1)、球销上盖(2-2)和中空球芯(2-4)，球销上盖(2-2)通过球销螺栓(2-3)将中空球芯(2-4)活动设置在球销座(2-1)上，球销座(2-1)通过螺栓固定在底基平台(1)上。

4.一种仿爬行类四足步行机器人运动参数的标定方法，其特征在于，其步骤如下：

步骤一：对仿爬行类四足步行机器人建立几何参数模型：设仿爬行类四足步行机器人的初始状态为矩形机体与腿主体立足点A_I、A_J、A_K、A_L所在的平面平行，髋关节的回转中心线垂直于机体平面，大腿关节的回转中心线与小腿关节的回转中心线平行于矩形机体平面，髋关节连接杆、大腿关节连接杆、小腿关节连接杆的结构对称面重合，在底基平台的投影在一条直线上；以仿爬行类四足步行机器人的矩形机体的中心C为原点、平行于矩形机体的边为坐标轴建立机体坐标系∑_C；设矩形机体的两边长度分别是2m和2n，E_C、F_C、G_C、D_C分别为矩形机体各边的中心，E_C、F_C、G_C、D_C在腿主体的立足点A_I、A_J、A_K、A_L所在平面的投影点分别是E_O、F_O、G_O和D_O，以F₀D₀和E₀G₀的交点O_O为原点、F₀D₀和E₀G₀所在直线为坐标轴建立基础坐标系Σ_o，则腿主体的立足点A_K,A_J,A_L,A_I在基础坐标系Σ_o中的坐标为腿主体从上到下的髋关节连接杆长为l₁、大腿关节连接杆长为l₂和小腿关节连接杆长为l₃，髋关节连接杆与大腿关节连接杆之间的夹角为大腿关节连接杆与小腿关节连接杆之间的夹角为χ_i，髋关节的旋转角φ_i，其中i＝I、J、K、L；

步骤二：在基础坐标系Σ_o上，以E₀、F₀、G₀和D₀为矩形机体在Σ_o中的对应投影矩形的各边的中点，作出投影矩形，投影矩形的各顶点分别为B_I、B_J、B_K、B_L；在投影矩形的各顶点B_I、B_J、B_K、B_L处分别连接相应的安装腿的链接球铰的中心位置A_I、A_J、A_K、A_L，再分别测量A_IB_I、A_JB_J、A_KB_K、A_LB_L的长度S_I、S_J、S_K、S_L，A_IB_I、A_JB_J、A_KB_K、A_LB_L与矩形边的角度φ_i，确定各腿主体的投影长度及投影相对与矩形机体的角度；

步骤三：测量矩形机体各边的中心E_C、F_C、G_C和D_C到基础坐标系Σ_o的距离分别为h_E、h_F、h_G和h_D；

步骤四：通过控制系统驱动仿爬行类四足步行机器人运动，矩形机体只作与初始姿态平行的运动，形成不同的运动姿态i₀；记录机器人的矩形机体各边的中心点E_C、F_C、G_C和D_C在基础坐标系Σ_o上的投影点和i₀＝1、2、3、4….n₀表示不同的位置变化，n₀为机器人位置变化的次数；矩形机体各边的中心点E_C、F_C、G_C和D_C到基础坐标系Σ_o的距离和测量各步态下矩形机体的中心投影在基础坐标系Σ_o中的坐标(0)和矩形机体绕基础坐标系Σ_o的z轴的偏转角

步骤五：通过测量数据计算仿爬行类四足步行机器人矩形机体绕基础坐标系∑_C的y轴运动的回转角偏差和绕x轴运动的俯仰角偏差并利用RPY模型描述爬行类四足步行机器人的方向矩阵R_c；

步骤六：利用偏转角偏差回转角偏差俯仰角偏差确定方向矩阵R_c的误差矩阵

步骤七：利用矩形机体的轨迹(^ox_c，^oy_c，^oz_c及误差(Δ^ox_c，Δ^oy_c，Δ^oz_c)矩形机体尺寸(m，n)及误差(Δm,Δn)、各腿主体立足点的轨迹及误差各腿关节的长度(l_1i，l_2i，l_3i)及误差(△l_1i，△l_2i，△l_3i)、各腿关节角度值(χi_，φ_i)及误差(Δχ_i，Δφ_i)、误差矩阵ΔR_c的关系建立参数方程；用参数方程求解腿主体关节的误差△l_1i、△l_2i、△l_3i和矩形机体尺寸的误差Δm、Δn；

步骤八：将腿关节的误差△l_1i、△l_2i、△l_3i和矩形机体尺寸误差Δm、Δn代入校验方程确定标定的结果是否满足精度要求。

5.根据权利要求4所述的一种仿爬行类四足步行机器人运动参数的标定方法，其特征在于，所述计算矩形机体绕基础坐标系Σ_o的z轴的偏转角偏差的方法是：以矩形机体各边的中点E_C、F_C、G_C和D_C在基础坐标系Σ_o上的投影点 和为各边的中点，组成投影矩形在矩形机体的投影矩形图上，采用将初始的图形E₀F₀G₀D₀平移坐标(0)获得新的图形则图形与图形两个中心线的夹角为偏转角偏差

6.根据权利要求4所述的一种仿爬行类四足步行机器人运动参数的标定方法，其特征在于，所述计算回转角偏差和俯仰角偏差的方法是：根据矩形机体各边的中心点E_C、F_C、G_C和D_C到基础坐标系Σ_o的距离与矩形机体的尺寸关系可知： 计算回转角偏差和俯仰角偏差