基于光学运动跟踪的服务机器人自动标定与检测系统的制作方法

本发明涉及服务机器人的自动标定与检测领域，尤其涉及一种基于光学运动跟踪的服务机器人自动标定与检测系统。

背景技术：

调整机器人运动学或动力学参数的过程通常叫做机器人标定。由于机器人零件的加工、装配过程中产生的误差或使用过程中造成的磨损，使得机器人模型参数的实际值和名义值之间存在差异，而模型误差的存在会导致后续行为决策和运动控制的误差。所以通常在机器人出厂或者工作之前，都会对机器人进行一定程度的检测和标定以提高控制精度，保证机器人的正常运行。

目前的标定方法主要包括外部标定和自标定两种。自标定主要利用机器人自身冗余的传感器信息来建立模型参数之间的限制关系，进而对参数进行求解和优化。这种方法的优势是不需要增加外部测量设备，但受限于自身传感器信息的误差和局部性，通常标定结果的精度有限。外部标定法利用外部测量设备得到待标定装置的实际测量值，同时利用参数模型来计算预期值，通过比较最小化测量值和预期值之间的误差，从而求解得到模型参数的实际值。外部设备能够获得误差很小的全局信息，因此外部标定法的结果比较精确。但由于外部测量工具的不同，相应的标定过程也不一样。而且已有的外部标定方法经常需要手动测量参数或人工干预标定流程，无法自动完成机器人的检测和标定，因此效率较低。

此外，实际运行中的机器人系统由于机械磨损或者外力作用，自身的结构会不断发生变化，造成机器人性能的下降。所以需要定期对机器人系统做性能检测和参数重标定。性能检测可以监控系统的性能变化，并以此为依据不断的对机器人系统的参数进行重新标定和校准，使其能够适应系统机构的变化。同时性能检测的结果提供了对机器人系统的诊断依据，能够发现系统设计中的缺陷或有瑕疵的零部件，从而保证机器人系统的实际性能能够满足设计的需求。

服务机器人作为机器人的一大类别，其设计目的和应用场合虽然不同于传统的工业机器人，但是服务机器人本身仍是一套极其复杂的电子机械机构。一台典型的服务机器人通常都具备感知、决策、控制、运动、操作等模块。这些模块的正常运行和协同工作都依赖众多的系统参数。而这些系统参数需要通过检测和标定过程来获得。

同时服务机器人的应用和普及又具有一些自身的特点：(1)需求数量巨大：随着劳动力成本的增加，社会对服务机器人的需求将不断提高，服务机器人的数量可能会出现爆炸式增长，而目前与之相对应的服务机器人生成制造过程中的检测和标定环节的效率普遍较低，无法满足服务机器人的巨大应用需求。(2)价格成本低：由于普通大众对服务机器人成本的承受能力有限，所以零件的加工和装配通常无法采用昂贵的高精度工艺，因此更加依赖生产后期的参数标定和性能检测来校正和调准。服务机器人的这些应用特点增加了在其生产制造及使用过程中对自动化程度高、通用性好的标定检测系统的需求。

然而，传统的标定和检测系统要求相关人员具备丰富的背景知识和经验，并且操作繁琐，严重依赖于人工参与，因此不能满足大批量的服务机器人生产制造。

技术实现要素：

本发明的目的是提供基于光学运动跟踪的服务机器人自动标定与检测系统，实现服务机器人标定与检测的自动化，从而提高效率。

本发明的目的是通过以下技术方案实现的：

一种基于光学运动跟踪的服务机器人自动标定与检测系统，包括：

光学运动跟踪系统MCS，其作为外部测量工具，能够对待标定服务机器人进行位姿跟踪，所测量到的数据作为外部测量数据传输给标定服务器；

待标定服务机器人，用于按照预定方式运行，并通过内部传感器记录相关数据，所记录的数据作为内部测量数据传输给标定服务器；

建模单元，用于建立待标定服务机器人的运动学模型，并传输给标定服务器；运动学模型描述的变量包括：待标定的参数、包含服务机器人内部测量数据的变量、包含服务机器人外部测量数据的变量，以及这些变量之间的函数关系；

标定服务器，用于收集外部测量数据和内部测量数据，并通过内部测量数据和模型关系，来计算预期值，并通过最小化预期值和实际测量值之间的误差平方和来确定运动学模型中待标定的参数；

性能检测单元，用于利用已确定待标定参数的运动模块来更新服务机器人的模型参数；然后，比较更新后模型的预期值和外部测量数据之间的误差，检测标定结果是否满足检测要求。

所述标定服务器内还包含：MCS Bridge单元、NTP时间同步单元及ROS Bridge单元；

所述MCS Bridge单元，用于将MCS系统发送的外部测量数据的格式转换成标定服务器所需格式；

ROS Bridge单元，用于将服务机器人发送的内部测量数据的格式转换成标定服务器所需格式；

NTP时间同步单元，用于负责MCS系统与服务机器人之间的时钟同步。

待标定服务机器人所记录的数据作为内部测量数据传输给标定服务器包括：

若内部测量数据量超过阈值，则认为数据量较大，内部测量数据将存储在待标定服务机器人内的离线数据单元中；若内部测量数据量未超过阈值，则认为数据量较小，内部测量数据通过网络传输给标定服务器内的ROS Bridge单元。

参数标定问题描述为：

y＝f(q,x,p)；

其中，q为内部测量数据，x为外部测量数据，p为运动学模型中待标定的参数，f为函数关系符；

上式为非线程方程，利用非线性求解的方法得到待标定的参数；

或者，利用微分使其线性化得到：

Δy(q,x,p)＝Ψ(q,x,p)·Δp；

上式中，Δp＝p_r-p，p_r为名义值，Δy为函数变化值，Ψ(q,x,y)为关于变量p的雅克比矩阵；

再通过求解上式得到待标定的参数。

由上述本发明提供的技术方案可以看出，实现了机器人自动标定与检测，利用该系统可以减少服务机器人标定和检测过程中的人工参与，提高效率。该系统具有非常好的通用性，对于不同的机器人，只需要输入对应的参数模型和设定的标定动作就可以方便的使用该系统。

附图说明

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

图1为本发明实施例提供的一种基于光学运动跟踪的服务机器人自动标定与检测系统的示意图；

图2为本发明实施例提供的一种基于光学运动跟踪的服务机器人自动标定与检测系统的工作过程示意图；

图3为本发明实施例提供的服务机器人的运行模型示意图。

具体实施方式

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

图1为本发明实施例提供的一种基于光学运动跟踪的服务机器人自动标定与检测系统的示意图；如图1所示，该系统主要包括：光学运动跟踪系统(MCS)、待标定服务机器人、建模单元、标定服务器以及性能检测单元；其中：

MCS系统，其作为外部测量工具，能够对待标定服务机器人进行位姿跟踪，所测量到的数据作为外部测量数据传输给标定服务器；

待标定服务机器人，用于按照预定方式运行，并通过内部传感器记录相关数据，所记录的数据作为内部测量数据传输给标定服务器；

建模单元，用于建立待标定服务机器人的运动学模型，并传输给标定服务器；运动学模型描述的变量包括：待标定的参数、包含服务机器人内部测量数据的变量、包含服务机器人外部测量数据的变量，以及这些变量之间的函数关系；

标定服务器，用于收集外部测量数据和内部测量数据，并通过内部测量数据和模型关系，来计算预期值，并通过最小化预期值和实际测量值之间的误差平方和来确定运动学模型中待标定的参数；

性能检测单元，用于利用已确定待标定参数的运动模块来更新服务机器人的模型参数；然后，比较更新后模型的预期值(即更新模型后的重新根据模型计算得到的数值)和外部测量数据之间的误差，检测标定结果是否满足检测要求。

本领域技术人员可以理解，MCS系统一次测量的数据最多包括六维，即刚体在三维空间的位置和姿态(x,y,z,roll,pitch,yaw)，但是不同的应用可能只能用到部分维度的数据，例如后文实施例中的里程计标定，因为机器人在地面平动，所以只有x坐标,y坐标,机器人朝向yaw这三维数据被用到。因此，具体到实际运用中此处的外部测量数据即为一个外部测量值。

对于不同的机器人配置，一次测量会得到该配置下的外部测量值和对应的模型预期值，这样就能计算出当前配置下的一个残差。

进一步的，所述标定服务器内还包含：MCS Bridge单元、NTP时间同步单元及ROS Bridge单元；

所述MCS Bridge单元，用于将MCS系统发送的外部测量数据的格式转换成标定服务器所需格式；

ROS Bridge单元，用于将服务机器人发送的内部测量数据的格式转换成标定服务器所需格式；

NTP时间同步单元，用于负责MCS系统与服务机器人之间的时钟同步。

进一步的，待标定服务机器人所记录的数据作为内部测量数据传输给标定服务器包括：

若内部测量数据量超过阈值，则认为数据量较大，内部测量数据将存储在待标定服务机器人内的离线数据单元中；若内部测量数据量未超过阈值，则认为数据量较小，内部测量数据通过网络传输给标定服务器内的ROS Bridge单元。

此外，参数标定问题描述为：

y＝f(q,x,p)；

其中，q为内部测量数据，x为外部测量数据，p为运动学模型中待标定的参数，f为函数关系符；

上式为非线程方程，利用非线性求解的方法得到待标定的参数；

或者，利用微分使其线性化得到：

Δy(q,x,p)＝Ψ(q,x,p)·Δp；

上式中，Δp＝p_r-p，p_r为已知的名义值，Δy为函数变化值，Ψ(q,x,y)为关于变量p的雅克比矩阵，通过求解上式得到待标定的参数。

为了便于理解，下面针对上述系统的工作过程做详细的说明。

如图2所示为，图1所示系统的工作过程示意图。从图2中来看，标定服务器主要的输入的包括：(1)模型：模型定义的是待标定的机器人的运动学模型，模型描述的变量主要分成三类：待标定的参数、包含机器人内部测量数据的变量、包含机器人外部测量数据的变量、以及这些变量之间的函数关系。(2)数据：输入的数据主要分成为两类：一类是外部测量数据，MCS所测量的数据；另外一类是服务器机器人内部传感器的测量数据。系统的输出主要是运动学模型中待求解参数的真实值。

本领域技术人员可以理解，建模单元建立的运动学模型可以采用常规方式实现；此外，MCS系统采集外部测量数据时，可以将机器人放置在测量区域内完成预先设计好的一套动作，再由MCS系统进行位姿跟踪，进而测量相应数据。

标定服务器内部还具格式转换单元(MCS Bridge单元与ROS Bridge单元)，可以分别对内、外部测量数据进行格式转换，以符合标定计算的要求；此外，标定检测所需的外部测量和内部测量数据必须是同一时刻产生的数据，但是由于MCS系统和服务机器人是相互孤立的系统，所以标定服务器内还设有NTP时间同步模块来负责保持MCS系统与服务机器人的时钟同步。

此外，由于服务机器人测量数据量的不同，传输给标定服务器的方式也不同；简单来说，如果数据量较少，那么可以直接通过网络，以在线传输的方式发送给标定服务器内的ROS Bridge单元；如果数据量较多，则相应的测量暂时保存在服务机器人本地，待测量结束后采用离线传输的方式发送至标定服务器。

标定服务器，根据接收到的这些数据结合标定算法进行运动学模型中待标定参数的计算，以计算其真实值。

当标定服务器计算出待标定参数的真实值后，还需要进行性能检测，以检测标定结果是否满足检测的要求

基于本发明实施例的上述方案，可以将待标定检测的服务机器人依次进入上述系统，该系统能够自动收集所需的测量数据。机器人在测量区域内完成预先设计好的一套动作，系统得到相关的数据并按照标定算法求解参数，进而完成标定检测过程。在该系统在其运行过程无需人工参与，能够自动完成批量的标定检测任务。

本发明实施例上述方案，可以应用于服务机器人批量生产过程、单台/套复杂机器人的组装生产过程或其它有标定需求的硬件系统制造过程；也可应用于对已有的服务机器人系统或其它硬件系统的性能检测和升级更新过程；也可应用于可拆卸系统、自组装系统等相关系统的自动标定。

示例性的，本发明提出的上述系统可以实现并应用于一种移动操作服务机器人的底盘标定上。该系统采用的MCS系统由12个内置FPGA处理芯片的智能红外摄像头组成，可对其覆盖范围内(20m²的测试区域)标记点进行持续的位姿跟踪，捕捉精度高达0.1mm。MCS系统能够为自动标定系统提供高频率，精确的外部测量数据，测量数据由网络交换机传输到局域网里面的标定服务器，机器人内部的传感器数据也通过网络实时传输到标定服务器。机器人进入测试区域按照预先设定的弧线轨迹运行，系统记录机器人对应的位姿测量信息和编码器数据，利用差动轮的模型，可以准确获取机器人的底盘左右轮的直径和轮间距。

该示例中，机器人的底盘驱动机构模型如图3所示，其位姿由两前驱动轮的运动组合而成。

设左右驱动轮的转速分别为ω_L和ω_R，机器人当前的角速度为ω，线速度为v，根据模型，则有：

其中参数矩阵C即为待标定参数，可以表示为：

上式中，b为两个驱动轮之间的距离，r_L和r_R为左右两个驱动轮的半径。

根据上面两个公式，可得到：

其中(x_k,y_k,θ_k),(x_k+1,y_k+1,θ_k+1)分别为k时刻和k+1时刻的机器人位姿(x方向坐标，y方向坐标，朝向θ)，T为采样间隔,w_k为机器人k时刻的角速度，v_k为机器人k时刻的线速度。

利用N个采样周期内机器人朝向的变化，可建立如下的线程关系：

其中θ₀和θ₀为起始时刻(0时刻)和结束时刻(N时刻)机器人的朝向，w_L,i和w_R,i为i时刻为左右驱动轮在(I,i+1)期间的转速，Φ_θ为系数向量。

执行P次采样过程并记录对应的数据，即可得到：

其中Φ_θ,p为第p次采样过程得到的系数向量，根据上式建立的线性关系，即可求解出参数矩阵C中的C_2，1和C_2，2。

同理，可以利用N采样周期内机器人位置变化的关系式，可求解出参数矩阵C中的C_1，1和C_1，2，上述C_1，1、C_1，2、C_2，1、C_2，2为参数矩阵C(2*2)中四个位置的元素。其中x₀,x_N为起始时刻(0时刻)和结束时刻(N时刻)机器人的x坐标，y₀,y_N为起始时刻(0时刻)和结束时刻(N时刻)机器人的y坐标，Φ_xy为系数向量。

标定开始后，将标记点固定在机器人上并进入标定场地绕圈运行，我们设置数据采集次数，系统自动采集数据并根据模型关系计算出里程计相关参数矩阵C。

再根据计算出的里程计相关参数更新服务机器人内的模型，并进行性能检测，以检测标定结果是否满足要求。

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