CN103722565A - 仿人机器人自碰撞监控***及监控方法 - Google Patents

Abstract

仿人机器人自碰撞监控***及监控方法，涉及机器人控制领域。本发明是为了提高机器人碰撞检测的精度和实时性保护仿人机器人的安全。本发明使用球扫掠凸体构建碰撞模型，离散自适应窗口角速度计算方法以及GJK算法，开发了一个独立于机器人任务规划和控制的快速高精度碰撞检测监控***，该***循环运行，以机器人关节角作为输入，判断机器人是否会发生碰撞，当碰撞时向机器人控制器发送急停指令。机器人安全运行时该***不影响机器人运行。本发明适用于机器人控制。

Description

仿人机器人自碰撞监控***及监控方法

技术领域

本发明涉及机器人控制领域。

背景技术

仿机器人的自碰撞包括机器人的身体和手臂，以及双臂/双腿的碰撞等。自碰撞的发生会损坏机器人，甚至伤到周围的工作人员。随着机器人自由度的增加，机器人发生自碰撞的可能性也将增大，因此解决这个问题对于拥有多自由度的仿人机器人是至关重要的，也是仿人机器人安全控制和执行任务的基础。

机器人碰撞检测***需要实时检测机器人自身的潜在碰撞，给出碰撞信息，计算分离构件的最短距离，有助于机器人的运动规划和安全控制。其中最短距离的信息可以使机器人和障碍物保持一定的距离，避开障碍。当包含大量需要检测的对象时，层次包围盒技术可加速对象的测试过程。利用体积略大而形状简单的包围盒包裹复杂的几何对象，首先进行包围盒的简单相交测试，快速地排除大量不相交的几何对象，再对剩余几何对象进行精确相交测试。层次包围盒技术可以选用不同的包围盒来平衡算法的计算效率，复杂程度，以及包裹几何对象的精确程度三者的关系。常用的包围盒包括：轴对齐包围盒（AABB包围盒），方向包围盒（OBB包围盒），包围球，球扫掠体和凸体。不同的层次包围盒技术有着各自的优缺点。例如：基于AABB包围盒的方法比基于OBB包围盒的方法更快，但OBB包围盒精度更高。这两种方法都是通过包围盒的相交测试来判断是否相交，不提供距离信息。包围球不受旋转变换的影响，能有效的检测碰撞和计算多物体间的最短距离，但包围球模型的精度较低。球扫掠体是由球体与对应的图元闵可夫斯基和形成的。根据对应图元的不同可分为不同的类型，例如：球扫掠直线（胶囊体）、球扫掠矩形（菱形体）以及球扫掠凸体。两个球扫掠体的相交测试是通过计算两个内部图元之间的距离和他们的半径之和比较来实现的。球扫掠体的测试消耗完全取决于对应距离的计算消耗。球扫掠直线广泛的被应用于多臂机构机器人的自碰撞检测，然而球扫掠直线只适用于圆柱关节，无法描述外形复杂的关节，限制了该方法的适用范围。凸体可以描述外形复杂的关节，具有良好的空间结构特性，可优化碰撞检测的效率，因此凸体也广泛的应用于碰撞检测。球扫掠凸体拥有凸体优点，比凸体数据结构更加简单，意味着球扫掠凸体在保证碰撞检测模型精度的前提下，可以提高碰撞检测的实时性。为了进一步提高机器人碰撞检测的精度和实时性保护仿人机器人的安全，本发明使用球扫掠凸体的碰撞模型开发了一个独立于机器人任务规划和控制的快速高精度碰撞检测监控***，该***循环运行，以机器人关节角作为输入，判断机器人是否会发生碰撞。

发明内容

本发明是为了提高机器人碰撞检测的精度和实时性保护仿人机器人的安全，从而提供一种仿人机器人自碰撞监控***及监控方法。

仿人机器人自碰撞监控***，它包括基于球扫略凸体的离散碰撞检测模块7、机器人运动状态预测模块6、关节角度速度计算模块5和通讯模块4；

通讯模块4用于将仿人机器人上的关节角速度传感器采集的关节角度信息传送给关节角速度计算模块5，还用于将基于球扫略凸体的离散碰撞检测模块7的在检测到碰撞时向机器人控制器2发送急停控制指令；

关节角度速度计算模块5用于根据仿人机器人关节位置传感器采集的关节角度信息和仿人机器人关节位置传感器的采样周期计算仿人机器人各关节的角速度；并将仿人机器人关节位置传感器采集的关节角度和获得的仿人机器人各关节的角速度发送给机器人运动状态预测模块6；

机器人运动状态预测模块6用于根据关节角度速度计算模块5发送的仿人机器人关节位置传感器采集的关节角度、仿人机器人各关节的角速度和自碰撞监控***引入的时延预测仿人机器人的运动状态；并将预测结果发送给球扫略凸体的离散碰撞检测模块7；

基于球扫略凸体的离散碰撞检测模块7用于构建仿人机器人的基于球扫掠凸体碰撞检测模型，并根据机器人运动状态预测模块6预测的机器人运动状态实时更新碰撞检测模型，优化碰撞对，并检测各碰撞对是否发生碰撞，并在发生碰撞时发出急停控制指令。

仿人机器人自碰撞监控方法，它包括基于球扫略凸体的离散碰撞检测的步骤、机器人运动状态预测的步骤、关节角度速度计算的步骤和通讯步骤；

基于球扫略凸体的离散碰撞检测的步骤：将仿人机器人上的关节角速度传感器采集的关节角度信息传送给关节角速度计算模块5，以及将基于球扫略凸体的离散碰撞检测模块7的在检测到碰撞时向机器人控制器2发送急停控制指令的步骤；

关节角度速度计算步骤：根据仿人机器人关节位置传感器采集的关节角度信息和仿人机器人关节位置传感器的采样周期计算仿人机器人各关节的角速度；并将仿人机器人关节位置传感器采集的关节角度和获得的仿人机器人各关节的角速度发送给机器人运动状态预测模块6的步骤；

机器人运动状态预测步骤：根据关节角度速度计算模块5发送的仿人机器人关节位置传感器采集的关节角度、仿人机器人各关节的角速度和自碰撞监控***引入的时延预测仿人机器人的运动状态；并将预测结果发送给球扫略凸体的离散碰撞检测模块7的步骤；

基于球扫略凸体的离散碰撞检测步骤：构建仿人机器人的基于球扫掠凸体碰撞检测模型，并根据机器人运动状态预测模块6预测的机器人运动状态实时更新碰撞检测模型，优化碰撞对，并检测各碰撞对是否发生碰撞，并在发生碰撞时发出急停控制指令的步骤。

关节角度速度计算步骤中，根据仿人机器人关节位置传感器采集的关节角度信息和仿人机器人关节位置传感器的采样周期计算仿人机器人各关节的角速度采用离散自适应窗口速度计算的方法实现。

关节角度速度计算步骤中，根据仿人机器人关节位置传感器采集的关节角度信息和仿人机器人关节位置传感器的采样周期计算仿人机器人各关节的角速度采用离散自适应窗口速度计算的方法实现，具体为：

选择窗口尺寸，所述窗口尺寸n=max{1,2,3...}，即满足下式中变量i的最大值；

$|{\mathrm{\α}}_i(t-i)-{\widehat{\mathrm{\α}}}_i(t-i)|\≤{\mathrm{\ϵ}}_i,\∀i\∈\{\mathrm{1,2},...,n\}$

式中：其中α_i(t-i)为t-i时刻关节传感器获得的关节角度，

是由自适应窗口速度计算方法获得的t-i时刻关节的角度，ε_i是两者之间所允许的最大误差，ε_i的大小取决于仿人机器人每个关节的最大角加速度；

α_i(t-i)由下式：

${\widehat{\mathrm{\α}}}_i(t-i)={\mathrm{\α}}_i\left(t\right)-\frac{i({\mathrm{\α}}_i\left(t\right)-{\mathrm{\α}}_i(t-n))}{n}$

获得；

其中：α(t)和α(t-n)分别为t时刻和t-n时刻传感器获得的关节角度；

当窗口尺寸确定后通过公式：

${\stackrel{.}{\mathrm{\α}}}_i\left(t\right)=\frac{{\mathrm{\α}}_i\left(t\right)-{\mathrm{\α}}_i(t-n)}{\mathrm{n\Δt}}$

计算仿人机器人各关节的角速度，其中：△t是仿人机器人关节位置传感器的采样周期。

基于球扫略凸体的离散碰撞检测中，构建仿人机器人的基于球扫掠凸体碰撞检测模型具体为：

使用球扫掠凸体数据结构来构建机器人碰撞检测模型，所述球扫掠凸体的数据结构定义为：

V(r;P)=convP+{b∈R³||b|≥r}

其中：convP是点集

形成的凸体，球扫掠凸体是由半径r为球体与对应点集P形成凸体的闵可夫斯基和。

基于球扫略凸体的离散碰撞检测中，检测各碰撞对是否发生碰撞的方法为：

使用Gilbert-Johnson-Keerthi算法计算每个碰撞对间的最短距离和最近点，当两球扫掠凸体的距离小于等于零时，则两物体发生碰撞。

基于球扫略凸体的离散碰撞检测的具体方法为：

步骤一、将预测后的机器人运动状态输入基于球扫掠凸体的离散碰撞检测模块，该模块首先要对监控的机器人模型进行预处理，将机器人各构件的pre模型转化为球扫掠凸体模型，对于非凸体构件，将其分解成多个凸体的组合，从而构建出静止的机器人碰撞模型；

步骤二、通过预测的机器人关节角度使用机器人正运动学计算出变换矩阵，从而算出机器人构件在运动时新的坐标位置，使机器人构件的碰撞模型随之运动；

步骤三、根据各构件运动范围，确定仿人机器人每个检测周期需要检测的碰撞对；

步骤四、使用GJK算法计算出各个碰撞对的最短距离和最近点位置，并三维显示机器人碰撞模型和可能的碰撞点；当检测到碰撞时，立即停止其他碰撞对的检测；使用通讯模块向机器人控制器发送急停指令。

本发明了保证了仿人机器人碰撞检测的精度，并能够实时性保护仿人机器人的安全，使碰撞检测的平均计算时间为0.7ms。

附图说明

图1是本发明***的结构示意图；标记1是机器人任务规划端，标记2是机器人控制器，标记3是仿人机器人。

图2是基于球扫掠凸体的离散碰撞检测模块的工作流程示意图；

具体实施方式

具体实施方式一、结合图1说明本具体实施方式，仿人机器人自碰撞监控***，它包括基于球扫略凸体的离散碰撞检测模块7、机器人运动状态预测模块6、关节角度速度计算模块5和通讯模块4；

通讯模块4用于将仿人机器人上的关节角速度传感器采集的关节角度信息传送给关节角速度计算模块5，还用于将基于球扫略凸体的离散碰撞检测模块7的在检测到碰撞时向机器人控制器2发送急停控制指令；

关节角度速度计算模块5用于根据仿人机器人关节位置传感器采集的关节角度信息和仿人机器人关节位置传感器的采样周期计算仿人机器人各关节的角速度；并将仿人机器人关节位置传感器采集的关节角度和获得的仿人机器人各关节的角速度发送给机器人运动状态预测模块6；

机器人运动状态预测模块6用于根据关节角度速度计算模块5发送的仿人机器人关节位置传感器采集的关节角度、仿人机器人各关节的角速度和自碰撞监控***引入的时延预测仿人机器人的运动状态；并将预测结果发送给球扫略凸体的离散碰撞检测模块7；

基于球扫略凸体的离散碰撞检测模块7用于构建仿人机器人的基于球扫掠凸体碰撞检测模型，并根据机器人运动状态预测模块6预测的机器人运动状态实时更新碰撞检测模型，优化碰撞对，并检测各碰撞对是否发生碰撞，并在发生碰撞时发出急停控制指令。

具体实施方式二、结合图2说明本具体实施方式，仿人机器人自碰撞监控方法，它包括基于球扫略凸体的离散碰撞检测的步骤、机器人运动状态预测的步骤、关节角度速度计算的步骤和通讯步骤；

通讯步骤：将仿人机器人上的关节角速度传感器采集的关节角度信息传送给关节角速度计算模块5，以及将基于球扫略凸体的离散碰撞检测模块7的在检测到碰撞时向机器人控制器2发送急停控制指令的步骤；

关节角度速度计算步骤：根据仿人机器人关节位置传感器采集的关节角度信息和仿人机器人关节位置传感器的采样周期计算仿人机器人各关节的角速度；并将仿人机器人关节位置传感器采集的关节角度和获得的仿人机器人各关节的角速度发送给机器人运动状态预测模块6的步骤；

机器人运动状态预测步骤：根据关节角度速度计算模块5发送的仿人机器人关节位置传感器采集的关节角度、仿人机器人各关节的角速度和自碰撞监控***引入的时延预测仿人机器人的运动状态；并将预测结果发送给球扫略凸体的离散碰撞检测模块7的步骤；

基于球扫略凸体的离散碰撞检测步骤：构建仿人机器人的基于球扫掠凸体碰撞检测模型，并根据机器人运动状态预测模块6预测的机器人运动状态实时更新碰撞检测模型，优化碰撞对，并检测各碰撞对是否发生碰撞，并在发生碰撞时发出急停控制指令的步骤。

原理：本发明目的在于针对现有技术的不足，使用球扫掠凸体构件碰撞模型，离散自适应窗口角速度计算方法以及GJK算法，开发了一个独立于机器人任务规划和控制的快速高精度碰撞检测监控***，该***循环运行，以机器人关节角作为输入，判断机器人是否会发生碰撞，当碰撞时向机器人控制器发送急停指令。

机器人安全运行时该***不影响机器人运行，进一步提高机器人碰撞检测的精度和实时性保护仿人机器人的安全。

仿人机器人自碰撞监控***包括通讯模块，关节角速度计算模块，机器人运动预测模块和基于球扫掠凸体的离散碰撞检测模块。

通讯模块周期性采样机器人各个关节位置传感器的关节角度信息和当自碰撞监控***检测到碰撞时向机器人控制器发送急停指令。

关节角速度计算模块根据机器人关节位置传感器的关节角度和通讯模块的采样周期计算机器人各关节的角速度。虽然可以使用两次采样的关节角度只差除以采样周期获得的关节角速度，但在实际应用中由于小的采样周期会放大测量误差，因此关节角速度测量模块使用了离散自适应窗口速度计算方法。该方法可以根据速度的大小自动选择窗口尺寸以保证计算结果的准确。当速度快时窗口尺寸将会变小，相反当速度慢时窗口的尺寸将会变大。

窗口尺寸n=max{1,2,3...}，即满足方程（1）变量i的最大值。

$|{\mathrm{\α}}_i(t-i)-{\widehat{\mathrm{\α}}}_i(t-i)|\≤{\mathrm{\ϵ}}_i,\∀i\∈\{\mathrm{1,2},...,n\}$

其中α_i(t-i)为t-i时刻传感器获得的关节角度，

是由自适应窗口速度计算方法获得的t-i时刻关节的角度，ε_i是两者之间所允许的最大误差，ε_i的大小取决于机器人每个关节的最大角加速度。

由下式获得：

${\widehat{\mathrm{\α}}}_i(t-i)={\mathrm{\α}}_i\left(t\right)-\frac{i({\mathrm{\α}}_i\left(t\right)-{\mathrm{\α}}_i(t-n))}{n}$

其中α(t)和α(t-n)分别为t时刻和t-n时刻传感器获得的关节角度。

当窗口尺寸确定后可通过下式计算出相应的关节的角速度。

${\stackrel{.}{\mathrm{\α}}}_i\left(t\right)=\frac{{\mathrm{\α}}_i\left(t\right)-{\mathrm{\α}}_i(t-n)}{\mathrm{n\Δt}}$

其中△t是通讯模块获得机器人关节位置传感器的采样周期。

直接使用关节位置传感器的信息进行自碰撞检测存在一定的滞后，不能保证机器人的安全，因此机器人运动预测模块假设在很短的检测周期内机器人各关节的速度变化可以忽略，根据关节角度，计算的关节角速度和自碰撞检测***引入的时延对当前机器人的运动状态进行预测，自碰撞监控***引入的时延包括通讯周期，自碰撞检测周期以及机器人控制周期可能引起的时间滞后。

基于球扫掠凸体的离散碰撞检测模块对模块3预测的机器人运动状态进行碰撞检测。由于自碰撞检测模块在每个检测周期都需要计算出碰撞模型中各点新的坐标。因此使用球扫掠凸体描述机器人各个构件，减少碰撞模型需要的数据量，增加碰撞检测的实时性。

球扫掠凸体的定义为：

V(r;P)=convP+{b∈R³||b|≥r}   (1)

其中，convP是点集

形成的凸体，球扫掠凸体是由半径r为球体与对应点集P形成凸体的闵可夫斯基和。

每个检测周期根据关节角度和机器人正运动学计算各个构件的变换矩阵

为前一检测周期构件i坐标系相对于世界坐标的变换矩阵，

为当前检测周期构件i坐标系相对于世界坐标的变换矩阵。而碰撞模型中各点新的坐标为^WP′_i=T^WP_i，^WP_i为前一个检测周期中i构件凸体点集的点相对世界坐标系的坐标，^WP′_i为当前检测周期i构件凸体点集的点相对世界坐标系的新坐标。

更新完碰撞模型后设置碰撞检测对，仿人机器人自碰撞监控***需要在每个检测周期内检测大量的碰撞对，因此优化碰撞对的对数可以提高机器人宇航员自碰撞检测的实时性和效率，通过考虑各构件运动范围进一步减少碰撞对数。

基于球扫掠凸体的离散碰撞检测模块使用GJK迭代算法对设置好的碰撞对进行判断。GJK算法是一种基于单形体的下降算法，它将两个凸体间的距离计算转化为求解单一凸体集内的距离，每次循环迭代中将搜索闵可夫斯基差对象，获取一个单形体子空间体。经过多次迭代后算法收敛于某一间距值，即两凸体的最短距离，计算两凸体的最近点。该算法输入任意两个凸体的顶点集，输出凸体间的欧式距离和最近的点。由于球扫掠凸体的基本图元是凸体，具备凸体的特性，因此针对凸体的GJK算法很容易应用到球扫掠凸体上。两球扫掠凸体的距离为对应的基本图元两凸体的的最短距离减去两扫掠球的半径。当两球扫掠凸体的距离小于等于零时，认为两物体发生碰撞。三维显示机器人碰撞检测模型和可能的碰撞点，发出警告，通过通讯模块向机器人控制器发送急停指令。

下面结合附图和具体的实施例对本发明作进一步的详细说明。：

使用C++语言编程实施本发明仿人机器人自碰撞监控***用于一个拥有19个构件的仿人机器人上，其具体步骤如下：

（1）使用基于UDP的Socket编程构建通讯模块，通过局域网周期性接受机器人控制器发送的机器人各个关节位置传感器的关节角度信息。

（2）关节角速度计算模块接受机器人关节位置传感器的关节角度和通讯模块的采样周期，使用离散自适应窗口速度计算方法，计算出窗口尺寸，从而算出各个关节的角速度。

（3）机器人运动预测模块假设在很短的检测周期内机器人各关节的速度变化可以忽略，根据位置传感器的关节角度，计算的关节角速度和自碰撞检测***引入的时延，对当前机器人的运动状态进行预测，当前机器人关节的运动状态为传感器的关节角度加上关节保持当前速度时延转过的角度。

（4）将预测后的机器人运动状态输入基于球扫掠凸体的离散碰撞检测模块。该模块首先要对监控的机器人模型进行预处理，将机器人各构件的pre模型转化为球扫掠凸体模型，对于非凸体构件，将其分解成多个凸体的组合。从而构建出静止的机器人碰撞模型。

（5）通过预测的机器人关节角度使用机器人正运动学计算出变换矩阵，从而算出机器人构件在运动时新的坐标位置，使机器人构件的碰撞模型也要随之运动。

（6）通过考虑各构件运动范围，如仿人机器人相邻关节的构件不发生碰撞；同侧手臂/腿不发生碰撞；头部不会和脚发生碰撞等减少碰撞对数。本实例中一个拥有19个构件的仿人机器人每个检测周期需要检测101对碰撞对。

（7）使用GJK算法计算出各个碰撞对的最短距离和最近点位置，并三维显示机器人碰撞模型和可能的碰撞点。当检测到碰撞时，***立即停止其他碰撞对的检测。使用通讯模块向机器人控制器发送急停指令。当无碰撞发生时，该***不会干涉机器人的正常运行。本实例中通信周期为50ms，基于球扫掠凸体的离散碰撞检测模块的检测周期为50ms，判断101对碰撞对的平均计算时间为0.7ms。

Claims (7)

1.仿人机器人自碰撞监控***，其特征是：它包括基于球扫略凸体的离散碰撞检测模块（7）、机器人运动状态预测模块（6）、关节角度速度计算模块（5）和通讯模块（4）；

通讯模块（4）用于将仿人机器人上的关节角速度传感器采集的关节角度信息传送给关节角速度计算模块（5），还用于将基于球扫略凸体的离散碰撞检测模块（7）的在检测到碰撞时向机器人控制器（2）发送急停控制指令；

关节角度速度计算模块（5）用于根据仿人机器人关节位置传感器采集的关节角度信息和仿人机器人关节位置传感器的采样周期计算仿人机器人各关节的角速度；并将仿人机器人关节位置传感器采集的关节角度和获得的仿人机器人各关节的角速度发送给机器人运动状态预测模块（6）；

机器人运动状态预测模块（6）用于根据关节角度速度计算模块（5）发送的仿人机器人关节位置传感器采集的关节角度、仿人机器人各关节的角速度和自碰撞监控***引入的时延预测仿人机器人的运动状态；并将预测结果发送给球扫略凸体的离散碰撞检测模块（7）；

基于球扫略凸体的离散碰撞检测模块（7）用于构建仿人机器人的基于球扫掠凸体碰撞检测模型，并根据机器人运动状态预测模块（6）预测的机器人运动状态实时更新碰撞检测模型，优化碰撞对，并检测各碰撞对是否发生碰撞，并在发生碰撞时发出急停控制指令。

2.仿人机器人自碰撞监控方法，其特征是：它包括基于球扫略凸体的离散碰撞检测的步骤、机器人运动状态预测的步骤、关节角度速度计算的步骤和通讯步骤；

通讯步骤：将仿人机器人上的关节角速度传感器采集的关节角度信息传送给关节角速度计算模块（5），以及将基于球扫略凸体的离散碰撞检测模块（7）的在检测到碰撞时向机器人控制器（2）发送急停控制指令的步骤；

关节角度速度计算步骤：根据仿人机器人关节位置传感器采集的关节角度信息和仿人机器人关节位置传感器的采样周期计算仿人机器人各关节的角速度；并将仿人机器人关节位置传感器采集的关节角度和获得的仿人机器人各关节的角速度发送给机器人运动状态预测模块（6）的步骤；

机器人运动状态预测步骤：根据关节角度速度计算模块（5）发送的仿人机器人关节位置传感器采集的关节角度、仿人机器人各关节的角速度和自碰撞监控***引入的时延预测仿人机器人的运动状态；并将预测结果发送给球扫略凸体的离散碰撞检测模块（7）的步骤；

基于球扫略凸体的离散碰撞检测步骤：构建仿人机器人的基于球扫掠凸体碰撞检测模型，并根据机器人运动状态预测模块（6）预测的机器人运动状态实时更新碰撞检测模型，优化碰撞对，并检测各碰撞对是否发生碰撞，并在发生碰撞时发出急停控制指令的步骤。

3.根据权利要求2所述的仿人机器人自碰撞监控方法，其特征在于关节角度速度计算步骤中，根据仿人机器人关节位置传感器采集的关节角度信息和仿人机器人关节位置传感器的采样周期计算仿人机器人各关节的角速度采用离散自适应窗口速度计算的方法实现。

4.根据权利要求2所述的仿人机器人自碰撞监控方法，其特征在于关节角度速度计算步骤中，根据仿人机器人关节位置传感器采集的关节角度信息和仿人机器人关节位置传感器的采样周期计算仿人机器人各关节的角速度采用离散自适应窗口速度计算的方法实现，具体为：

选择窗口尺寸，所述窗口尺寸n=max{1,2,3...}，即满足下式中变量i的最大值；

$|{\mathrm{\α}}_i(t-i)-{\widehat{\mathrm{\α}}}_i(t-i)|\≤{\mathrm{\ϵ}}_i,\∀i\∈\{\mathrm{1,2},...,n\}$

式中：其中α_i(t-i)为t-i时刻关节传感器获得的关节角度，

是由自适应窗口速度计算方法获得的t-i时刻关节的角度，ε_i是两者之间所允许的最大误差，ε_i的大小取决于仿人机器人每个关节的最大角加速度；

α_i(t-i)由下式：

${\widehat{\mathrm{\α}}}_i(t-i)={\mathrm{\α}}_i\left(t\right)-\frac{i({\mathrm{\α}}_i\left(t\right)-{\mathrm{\α}}_i(t-n))}{n}$

获得；

其中：α(t)和α(t-n)分别为t时刻和t-n时刻传感器获得的关节角度；

当窗口尺寸确定后通过公式：

${\stackrel{.}{\mathrm{\α}}}_i\left(t\right)=\frac{{\mathrm{\α}}_i\left(t\right)-{\mathrm{\α}}_i(t-n)}{\mathrm{n\Δt}}$

计算仿人机器人各关节的角速度，其中：△t是仿人机器人关节位置传感器的采样周期。

5.根据权利要求2所述的仿人机器人自碰撞监控方法，其特征在于基于球扫略凸体的离散碰撞检测步骤中，构建仿人机器人的基于球扫掠凸体碰撞检测模型具体为：

使用球扫掠凸体数据结构来构建机器人碰撞检测模型，所述球扫掠凸体的数据结构定义为：

V(r;P)=convP+{b∈R³||b|≥r}

其中：convP是点集形成的凸体，球扫掠凸体是由半径r为球体与对应点集P形成凸体的闵可夫斯基和。

6.根据权利要求2所述的仿人机器人自碰撞监控方法，其特征在于基于球扫略凸体的离散碰撞检测步骤中，检测各碰撞对是否发生碰撞的方法为：

使用Gilbert-Johnson-Keerthi算法计算每个碰撞对间的最短距离和最近点，当两球扫掠凸体的距离小于等于零时，则两物体发生碰撞。

7.根据权利要求2所述的仿人机器人自碰撞监控方法，其特征在于基于球扫略凸体的离散碰撞检测的具体方法为：

步骤一、将预测后的机器人运动状态输入基于球扫掠凸体的离散碰撞检测模块，该模块首先要对监控的机器人模型进行预处理，将机器人各构件的pre模型转化为球扫掠凸体模型，对于非凸体构件，将其分解成多个凸体的组合，从而构建出静止的机器人碰撞模型；

步骤二、通过预测的机器人关节角度使用机器人正运动学计算出变换矩阵，从而算出机器人构件在运动时新的坐标位置，使机器人构件的碰撞模型随之运动；

步骤三、根据各构件运动范围，确定仿人机器人每个检测周期需要检测的碰撞对；

步骤四、使用GJK算法计算出各个碰撞对的最短距离和最近点位置，并三维显示机器人碰撞模型和可能的碰撞点；当检测到碰撞时，立即停止其他碰撞对的检测；使用通讯模块向机器人控制器发送急停指令。

Images