1 引言(Introduction)

随着机器人技术的发展，其应用从工业制造越来越多地进入到家庭服务、医疗护理、航天科技等诸多领域^[1-3]。工作环境结构逐渐由传统单一型环境转变为复杂非结构化环境。在过去的几年中，机器人的应用方式已经发生了变化，从机器人单独工作的无人环境到机器人与人协同进行作业的场景^[4-7]。当人与机器人共享同一工作空间时，如何判断机器人和人的位姿状态，从而有效避免人与机器人碰撞是人机交互中需要解决的关键难题之一^[6-10]。

现有的人机碰撞检测方法大部分是基于工作空间的检测，当检测到人类靠近时，机器人会停止运动或降低速度。标准ISO10218和技术规范TS15066定义了协作机器人的安全要求^[8]。根据人的位姿信息可以定义适当的人—机安全距离，并选择合适的碰撞检测方法^[9-10]。目前，获取人体手臂位姿信息的传感器主要有基于视觉的传感器(例如激光扫描仪、光学3维动作捕捉系统等)和可穿戴式传感器(例如磁性传感器、惯性传感器、表面肌电信号sEMG(surface electromyography)采集器等)^[11-12]。基于视觉的传感器具有非接触性和较高的准确性，但是容易受到环境的影响。磁性传感器可精确测量人体运动，但对电磁噪声非常敏感，且需要将其附着在人体上。惯性传感器是相对便宜的可穿戴设备，可以在混乱的环境中使用。但是，由于其存在漂移现象，长时间使用会缺乏准确性^[12]。sEMG能够一定程度地反映人体各肌肉的信息，但具有较大的个体差异性^[13]。

单一传感信息有时很难反映非结构化环境的特征，部分学者对基于多传感器信息融合的人机位姿估计进行了研究。文[14] 通过光学3维动作捕捉系统和sEMG信息提出一种结合连续小波变换(CWT)和反向传播神经网络(BPNN)的方法来估计人体手臂肘关节的角度和速度。文[15] 结合光学3维动作捕捉系统和sEMG，将肌电信号的特征作为输入，通过神经网络获取关节角度，从而获得数据特征到关节角度的映射。文[16] 通过激光扫描仪和Kinect传感器重建了移动机器人周围环境信息。文[17] 则通过深度传感器和激光扫描仪来获取人与机器人的位姿状态，从而估计人与机器人的相对距离。上述文献中所提出的方法均可以较好地估计人与机器人的相对位姿。但是，在人机交互的复杂非结构化环境中，可能会出现因遮挡导致相关信息丢失的情况，从而影响人机位姿估计和碰撞检测的结果。文[18] 提出了一种基于粒子滤波的人体姿态估计算法，该方法可在人体被局部遮挡的条件下估计人体姿态。文[19] 基于卡尔曼滤波器和最大后验概率目标匹配方法实现了目标的连续跟踪。文[20] 利用改进的分层粒子滤波器来跟踪人体手部运动。文[21] 基于粒子群优化算法，利用多个视角的摄像机所拍摄的视频序列来解决全身关节运动跟踪问题。文[6] 结合光学3维动作捕捉系统和惯性传感器对人体在遮挡情况下的运动信息进行估计。然而，这些方法都采用的是惯性传感器，其自身存在漂移现象，故无法在长时间的人机协作中获取准确的人体运动信息。

在人机协作碰撞检测的研究中，常使用球体^[22]、胶囊体^{[9, 23-24]} 和圆柱体^[25-28] 等包围盒来构建人与机器人的碰撞检测模型。文[22] 用球体描述人与机器人，机器人通过一组预定义的搜索方向进行搜索和优化，最终选择出一条无碰撞的最优路径。文[9] 用胶囊体来描述人与机械臂，结合惯性测量单元(IMU)和激光扫描数据实现在线碰撞检测。文[25] 采用圆柱体描述不同的物体，并计算圆柱体之间的最小距离，以避免碰撞。文[26] 提出以12个边界框来描述空间状态下的机械臂，并规划出了一条无碰撞的运动轨迹。文[27] 提出了一种能确定相机图像中多个已知对象与多个未知对象之间的最小距离的方法，通过对物体进行投影并判断是否与其他未知对象区域相交来进行碰撞检测。文[28] 利用圆柱体表示机械臂，提出了一种将3维碰撞转变为2维碰撞图的方法。文[29] 提出了一种利用多个深度相机计算物体之间最短距离的方法。文[30] 利用3D点云将面向深度空间的笛卡儿空间离散化，该方法利用网格可以精确描述机器人及其周围环境，但是采用该方法计算机器人与障碍物之间最短距离的成本很高。胶囊体相较于上述方法具有计算成本低、位姿描述准确等优点^[24]。

本文结合光学3维动作捕捉系统与角度传感器，提出了一种人与机器人共存中的人机位姿估计与碰撞检测方法。首先利用光学3维动作捕捉系统获取人体手臂标记点信息，建立人体手臂的运动学模型。其次，针对运动捕捉系统存在的标记点信息缺失问题，结合角度传感器信息估计人体手臂末端位姿。再次，通过胶囊体构建人体手臂和机器人的碰撞检测模型，计算人机之间的最短距离。最后，通过人机协同作业实验验证本文所提位姿估计与碰撞检测方法的有效性。

2 人与机器人共存中的位姿估计与碰撞检测(Pose estimation and collision detection in human-robot coexistence)

图 1为本文提出的人与机器人共存中的位姿估计与碰撞检测方法。首先，利用动作捕捉系统采集人体手臂的位姿信息，建立人体手臂运动学模型。接着，判断动作捕捉系统的标记点信息是否丢失。当标记点丢失时，结合角度传感器估计人体末端位姿，从而建立人体手臂的胶囊体模型。当标记点未丢失时，则直接根据动作捕捉系统获取的标记点信息建立人体手臂的胶囊体模型。进一步地，根据机器人位姿信息建立机器人胶囊体模型。计算描述人体手臂的胶囊体和机器人的胶囊体之间的最短距离。最后，根据人机最短距离判断是否发生碰撞。

2.1 基于动作捕捉系统和角度传感器的人体手臂位姿估计

光学3维动作捕捉系统相较于其他的室内定位技术具有精度高、实时性好等优点。本文采用NOKOV公司的光学3维动作捕捉系统分析手臂前臂和上臂的运动特性。该系统采用8个型号为Mars2H、分辨率为2048$ \times $1088、帧率为340 Hz的红外高速摄像机，系统定位精度为0.02 mm^[31-32]。通过不同相机对同一个标记点的空间运动图像进行采集，标记点反射回的红外光通过信号处理后由现场可编程门阵列(FPGA)进行图像抓取和算法处理，获得反光标记点在镜头中的2维坐标。多个镜头通过L形标定框确立空间原点，接着通过T形标定框标定系统工作空间，从而获取每个镜头的具体位置，最终得到标记点的3维坐标。根据获取的3维坐标进行相关性分析，能够在工作空间中捕捉贴有标记点的移动物体的运动。

本文将手臂简化为两连杆结构，在肩关节、肘关节及腕关节上放置5个标记点$ A $、$ B $、$ D $、$ C $、$ E $，各点在光学3维动作捕捉系统坐标系中坐标分别为$ (x_{A}, $ $ y_{A}, $ $ z_{A}) $、$ (x_{B}, $ $ y_{B}, $ $ z_{B}) $、$ (x_{C}, $ $ y_{C}, $ $ z_{C}) $、$ (x_{D}, $ $ y_{D}, $ $ z_{D}) $、$ (x_{E}, $ $ y_{E}, $ $ z_{E}) $。各点放置位置如图 3(a) 所示。图 3(b) 中$ O_{\rm{u}} $-$ X_{\rm{u}}Y_{\rm{u}}Z_{\rm{u}} $表示人体上臂坐标系，$ O_{\rm{f}} $-$ X_{\rm{f}}Y_{\rm{f}}Z_{\rm{f}} $表示人体前臂坐标系。$ \theta _{1} $表示竖直状态下两坐标系间的角度偏差。

图 4表示简化后的人体手臂两连杆模型，其中，$ {{{\mathit{\boldsymbol{ n }}}}}_{1} $和$ {{{\mathit{\boldsymbol{ n }}}}}_{2} $分别表示$ B $、$ E $标记点到上臂杆向量$ {{\mathit{\boldsymbol{ L }}}}_{1} $和前臂杆向量$ {{{\mathit{\boldsymbol{ L }}}}}_{2} $的垂直向量。$ F $点表示人体手臂手部末端，向量$ {{{\mathit{\boldsymbol{ L }}}}}_{3} $和前臂杆向量$ {{{\mathit{\boldsymbol{ L }}}}}_{2} $同向(即可理解为将$ {{{\mathit{\boldsymbol{ L }}}}}_{2} $延长至手部末端)。$ \theta $表示肘关节的关节角度：

$ \begin{equation} \theta =\arccos \frac{{{{\mathit{\boldsymbol{ L }}}}}_{1} \cdot {{{\mathit{\boldsymbol{ L }}}}}_{2} }{\left| {{{{\mathit{\boldsymbol{ L }}}}}_{1}} \right|\cdot \left| {{{{\mathit{\boldsymbol{ L }}}}}_{{\rm 2}}} \right|} \end{equation} $

(1)

其中，$ {{{\mathit{\boldsymbol{ L }}}}}_{1}=[x_{A}- x_{C}, y_{A}-y_{C}, z_{A}-z_{C}] $, $ {{{\mathit{\boldsymbol{ L }}}}}_{2}=[x_{C}-x_{D}, y_{C}-y_{D}, $ $ z_{C}-z_{D}] $，$ \theta \in (0, \pi ) $。然而，在非结构化环境下，$ D $、$ E $两点容易被遮挡，导致前臂位姿信息丢失。针对这一问题，本文通过人体上肢运动学模型结合角度传感器的方法来估计人体手臂末端的位姿。

如图 5所示，$ O_{0} $和$ O_{1} $所在的位置为图 3(a) 中$ A $点和$ D $点的位置，$ O_{2} $则表示人体上肢手部末端的位置。上臂姿态的坐标系定义为$ O_{0} $-$ X_{0}Y_{0}Z_{0} $，前臂姿态的坐标系定义为$ O_{1} $-$ X_{1}Y_{1}Z_{1} $。$ \theta $为肘关节的关节角度，$ a $表示图 3(a) 中$ A $点到$ C $点的距离(记为人体上臂长度)，$ b $表示图 3(a) 中$ C $点到$ D $点的距离(记为人体前臂长度)，$ c $表示人体上肢手腕到手部末端的距离。

动作捕捉系统可以根据图 3(a) 中$ A $、$ B $和$ C $三点得出上臂坐标系相对于大地坐标系的欧拉角，用$ ZYX $欧拉坐标变换来描述人体上臂的姿态，大地坐标系先绕$ Z $轴旋转$ \alpha $，再绕$ Y $轴旋转$ \beta $，最后绕$ X $轴旋转$ \gamma $。设$ {{{\mathit{\boldsymbol{ R }}}}}^{\rm{world}}_{\rm{arm}} $为大地坐标系到人体上臂坐标系的变换矩阵，可得：

$ \begin{align} {{{\mathit{\boldsymbol{ R }}}}}_{{\rm arm}}^{{\rm world}} &= {{{\mathit{\boldsymbol{ R }}}}}_{Z} (\alpha ){{{\mathit{\boldsymbol{ R }}}}}_{Y} (\beta) {{{\mathit{\boldsymbol{ R }}}}}_{X} (\gamma) \\ &= \begin{bmatrix} {{ c}_{\alpha} { c}_{\beta}} & {{ c}_{\alpha} { s}_{\beta} { s}_{\gamma} -{ s}_{\alpha} { c}_{\gamma}} & {{ c}_{\alpha} { s}_{\beta} { s}_{\gamma} +{ s}_{\alpha} { s}_{\gamma}} \\ {{ s}_{\alpha} { c}_{\beta}} & {{ s}_{\alpha} { s}_{\beta} { s}_{\gamma} +{ c}_{\alpha} { c}_{\gamma}} & {{ s}_{\alpha} { s}_{\beta} { c}_{\gamma} -{ c}_{\alpha} { s}_{\gamma}} \\ {-{ s}_{\beta}} & {{ c}_{\beta} { s}_{\gamma}} & {{ c}_{\beta} { c}_{\gamma}} \\ \end{bmatrix}\!\!\! \end{align} $

(2)

其中$ c_\alpha $表示$ \cos \alpha $，$ s_\alpha $表示$ \sin \alpha $，其余依此类推。设$ {{\mathit{\boldsymbol{ t }}}}_{0} = [x_{A}, $ $ y_{A}, $ $ z_{A}]^{\rm T} $，大地坐标系到人体上臂坐标系的齐次变换矩阵$ {{{\mathit{\boldsymbol{ T }}}}}^{\rm{world}}_0 $为

$ \begin{equation} {{{\mathit{\boldsymbol{ T }}}}}_{0}^{{\rm world}} = \begin{bmatrix} {{{\mathit{\boldsymbol{ R }}}}}_{{\rm arm}}^{{\rm world}} & {{{\mathit{\boldsymbol{ t }}}}_{0}} \\ 0 & 1 \\ \end{bmatrix} \end{equation} $

(3)

由于人为贴点可能会造成人体手臂在竖直状态下上臂坐标系和前臂坐标系不平行，因此，根据图 3(b) 可知竖直状态下两坐标系间的角度偏差为$ \theta_{1} $。从坐标系$ O_{0} $-$ X_{0}Y_{0}Z_{0} $变换到坐标系$ O_{1} $-$ X_{1}Y_{1}Z_{1} $需要加入补偿值$ {{{\mathit{\boldsymbol{ T }}}}}_{\rm{offset}} $：

$ \begin{equation} {{\mathit{\boldsymbol{T}}}}_{{\rm{offset}}} =\begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & {\cos \theta_{1}} & {-\sin \theta_{1}} & 0 \\ 0 & {\sin \theta_{1}} & {\cos \theta_{1}} & 0 \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} \end{equation} $

(4)

由图 5得坐标系$ O_{0} $-$ X_{0}Y_{0}Z_{0} $变换到坐标系$ O_{1} $-$ X_{1}Y_{1}Z_{1} $的齐次变换矩阵$ {{{\mathit{\boldsymbol{ T }}}}}^0_1 $为

$ \begin{equation} {{{\mathit{\boldsymbol{ T }}}}}_{1}^{0} =\begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & {\cos \theta} & {-\sin \theta} & {b\sin \theta} \\ 0 & {\sin \theta} & {\cos \theta} & {b\cos \theta +a} \\ 0 & 0 & 0 & 1 \\ \end{bmatrix} \end{equation} $

(5)

根据式(2)~式(5) 可计算出大地坐标系到人体前臂坐标系的齐次变换矩阵$ {{{\mathit{\boldsymbol{ T }}}}}^{\rm{world}}_1 $：

$ \begin{align} {{{\mathit{\boldsymbol{ T }}}}}_{1}^{{\rm world}}& ={{{\mathit{\boldsymbol{ T }}}}}_{0}^{{\rm world}} {\mathit{\boldsymbol{T}}}_{{\rm{offset}}} {{{\mathit{\boldsymbol{ T }}}}}_{1}^{0} \\ &=\begin{bmatrix} {{{\mathit{\boldsymbol{ R }}}}_{1}} & {{{\mathit{\boldsymbol{ t }}}}_{1}} \\ 0 & 1 \\ \end{bmatrix} \end{align} $

(6)

其中，$ {{{\mathit{\boldsymbol{ R }}}}_{1}} $和$ {{{\mathit{\boldsymbol{ t }}}}_{1}} $分别为大地坐标系到前臂坐标系的旋转矩阵和平移矩阵。由式(6) 可求出前臂末端$ O_{1} $在世界坐标系中的位姿。

同理，将式(5) 中参数$ b $更改为$ b+c $即可求出手部$ O_{2} $末端的姿态。

2.2 基于胶囊体的碰撞检测

获取人体手臂的位姿信息后，为了保证人机交互的安全性，需要知道人—机之间的最短距离。由图 6可知，本文将人体手臂简化为3个胶囊体(前臂、上臂和手部)，6自由度机械臂(UR5)简化为如图所示的4个胶囊体，求出表示人体手臂的3个胶囊体与表示UR5的4个胶囊体的最短距离为人体手臂到UR5的最短距离。

如图 7所示，计算2个胶囊体之间的最短距离可以简化为计算其轴线之间的最短距离。其中每个胶囊可以由2个向量(分别指向胶囊轴段的开始和结束位置)和半径$ \rho $定义。

用$ {{{\mathit{\boldsymbol{ p }}}}}_{1} $、$ {{{\mathit{\boldsymbol{ u }}}}}_{1} $(胶囊1)和$ {{{\mathit{\boldsymbol{ p }}}}}_{2} $、$ {{{\mathit{\boldsymbol{ u }}}}}_{2} $ (胶囊2)分别表示胶囊轴段端点的位姿向量，因此图中胶囊体的轴线向量分别为$ {{{\mathit{\boldsymbol{ s }}}}}_{1} = {{{\mathit{\boldsymbol{ u }}}}}_{1}-{{{\mathit{\boldsymbol{ p }}}}}_{1} $和$ {{{\mathit{\boldsymbol{ s }}}}}_{2}= {{{\mathit{\boldsymbol{ u }}}}}_{2}-{{{\mathit{\boldsymbol{ p }}}}}_{2} $。$ {{{\mathit{\boldsymbol{ r }}}}}_{1} $和$ {{{\mathit{\boldsymbol{ r }}}}}_{2} $表示2个胶囊体轴线上最近距离的点的坐标，则有：

$ \begin{equation} {{{\mathit{\boldsymbol{ r }}}}}_{1} ={{{\mathit{\boldsymbol{ p }}}}}_{1} +\lambda_{1} {{{\mathit{\boldsymbol{ s }}}}}_{1}, \quad {{{\mathit{\boldsymbol{ r }}}}}_{2} ={{{\mathit{\boldsymbol{ p }}}}}_{2} +\lambda_{2} {{{\mathit{\boldsymbol{ s }}}}}_{2} \end{equation} $

(7)

其中$ \lambda_{1} \in (0, 1) $，$ \lambda_{2} \in (0, 1) $，则最短距离函数$ f $为

$ \begin{align} \min (f)&=\min (\left| {{{{\mathit{\boldsymbol{ r }}}}}_{1}} \right.-\left. {{{{\mathit{\boldsymbol{ r }}}}}_{2}} \right|) \\ &= \min (({{{\mathit{\boldsymbol{ A }}}}{{\mathit{\boldsymbol{ x }}}}}+{{{\mathit{\boldsymbol{ y }}}}})^{{\rm T}}({{{\mathit{\boldsymbol{ A }}}}{{\mathit{\boldsymbol{ x }}}}}+{\mathit{\boldsymbol{y}}})) \end{align} $

(8)

其中矩阵$ {{{\mathit{\boldsymbol{ A }}}}} ={{{\mathit{\boldsymbol{ s }}}}}_{2}-{{{\mathit{\boldsymbol{ s }}}}}_{1} $，$ {{{\mathit{\boldsymbol{ x }}}}} = [ {{{ \lambda}} _{1}} \; \; {{{ \lambda}} _{{2}}}]^{\rm T} $，$ {{{ \lambda}} _{{1}}} $和$ {{{ \lambda}} _{{2}}} $为0~1之间的随机变量，向量$ {{{\mathit{\boldsymbol{ y }}}}} = {{{\mathit{\boldsymbol{ p }}}}}_{2}-{{{\mathit{\boldsymbol{ p }}}}}_{1} $。采用矩阵$ {{{\mathit{\boldsymbol{ Q }}}}} $和$ {{{\mathit{\boldsymbol{ R }}}}} $因式分解矩阵$ {{{\mathit{\boldsymbol{ A }}}}} $，则：

$ \begin{align} \min f &=\min (({\mathit{\boldsymbol{Rx}}}+{{{\mathit{\boldsymbol{ Q }}}}}^{\rm T}{{{\mathit{\boldsymbol{ y }}}}})^{\rm T}({\mathit{\boldsymbol{Rx}}}+{{{\mathit{\boldsymbol{ Q }}}}}^{\rm T}{{{\mathit{\boldsymbol{ y }}}}})) \\ &= \min ({{{\mathit{\boldsymbol{ u }}}}}^{\rm T}{{{\mathit{\boldsymbol{ u }}}}}) \end{align} $

(9)

其中，$ {{{\mathit{\boldsymbol{ Q }}}}} $是一个3$ \times $2的单位正交矩阵，$ {{{\mathit{\boldsymbol{ R }}}}} $是一个2$ \times $2的上三角矩阵。2个胶囊体的最短距离$ d_{\min} $为

$ \begin{equation} d_{\min} =\sqrt{\min f +{{{\mathit{\boldsymbol{ y }}}}}^{{\rm T}}{{{\mathit{\boldsymbol{ y }}}}}-{\mathit{\boldsymbol{y}}}^{{\rm T}}{\mathit{\boldsymbol{QQ}}}^{{\rm T}}{{{\mathit{\boldsymbol{ y }}}}}}-\rho_{1} -\rho _{2} \end{equation} $

(10)

其中，$ \rho_{1} $和$ \rho_{2} $表示2个胶囊体中圆柱体的半径。

3 实验与讨论(Experiment and discussion) 3.1 不同传感器测量的关节角度对比

在人体手臂前臂被遮挡情况下，需要获取肘关节关节角度来估计人体手臂末端姿态。为验证角度传感器的有效性，本文将角度传感器(型号为SG110)和IMU(型号为LPMS-B2)进行对比。其中，要求测试者按照图 8所示站在动作捕捉系统工作空间中，保持手臂其他部位不动，只运动肘关节让前臂上下摆动。动作捕捉系统、IMU和角度传感器均以100 Hz的频率来测量肘关节角度的变化，肘关节角度的对比结果如图 9和表 1所示。

表 1将图 9中动作捕捉系统测量的关节角度作为真实值，继而计算出角度传感器和IMU获得的关节角度相对于动作捕捉系统获得的关节角度之间的误差。从表中可以得出角度传感器相较于IMU精度提升了2.27$ ^{\circ} $，且最大误差减小了5.88$ ^{\circ} $。因此，在前臂信息丢失的情况下，角度传感器相比IMU能获得更加准确的关节角度。

3.2 人体上臂末端位姿估计实验

本文实验由10名身心健康的志愿者(男性9人，女性1人，25 $ \pm $ 5岁)参与。受试者按照图 10所示进行运动，其中，M1表示受试者用右手触摸右肩矢状面(保持肩关节不动，肘关节运动)，M2表示受试者在矢状平面内将其右臂抬起(保持肘关节不动，肩关节运动)，M3表示受试者在冠状平面内将其右臂抬起(保持肘关节不动，肩关节运动)，M4表示受试者用右手将地面上的工具放置到工作台上。在实验中，要求受试者按照图 10所示红线轨迹往复运动10 s。

图 11中红线表示动作捕捉系统测量的人体手臂末端轨迹，蓝线表示基于角度传感器和手臂运动学模型估计的人体手臂末端轨迹。各受试者在不同运动轨迹下的误差如图 12所示。

实验表明，本文方法的M1动作轨迹误差较大，原因是角度传感器需要紧贴在人体手臂，而肘关节在关节角度较大时肌肉紧绷，角度传感器相较人体手臂发生了一定偏移，导致轨迹误差增大。然而在M2、M3和M4动作中，肘关节的关节角度较小，肌肉并未紧绷，因此轨迹误差相较于M1动作较小。整体上看，结合角度传感器估计的手臂末端轨迹与动作捕捉系统获得的末端轨迹接近，个体差异较小，误差在20 mm以内，具有较高的精确性。因此，结合角度传感器信息可在标记点被遮挡情况下较准确地描述人体手臂末端的位姿信息。

3.3 人机碰撞检测实验

图 13为受试者与机器人的碰撞实验。其中，(a) 表示UR5机器人沿$ x $轴方向往复运动时，人体手臂在4~6 s时间段内与其发生碰撞的过程；(b) 表示人体手臂不动，UR5机器人沿图示$ y $轴方向往复运动；(c) 表示在UR5机器人沿$ y $轴方向往复运动，人体手臂在10 ~40 s时间段内与其发生碰撞的过程；(d) 表示UR5机器人不动，人体上肢手部末端与UR5机器人发生碰撞的过程。图 14为在图 13所示运动下的人机碰撞检测实验结果。

图 13中红线表示动作捕捉系统捕捉人体手臂末端标记点计算得到的人机最短距离，蓝线表示利用本文方法估计出人体手臂末端位姿后计算获得的人机最短距离，绿线表示人机距离为0 mm时的检测线。以红线作为人机距离的真实值，估计值与真实值的最大误差如图 15所示。

结果表明，通过本文所提方法计算获得的人机最短距离与通过动作捕捉系统计算获得的人机最短距离相似。当人体手臂与机械臂发生物理接触时，通过本文所提方法计算获得的人机最短距离接近0，说明该方法能够准确地实现碰撞检测。

接着，本文做了前臂末端标记点被遮挡后的人机碰撞检测实验。受试者按照图 16所示的动作顺序进行运动，在2~3.5 s时间段内手臂末端标记点被遮挡。图 17表示该运动下人体手臂前臂末端标记点被遮挡前后人体手臂与机械臂的人机最短距离，图中蓝线表示标记点被遮挡后通过本文方法估计的人机最短距离，红线表示标记点被遮挡前通过动作捕捉系统获得的人体手臂末端到机器人的最短距离。

实验结果表明，本文方法可以及时估计出人体手臂末端的位姿，并进一步计算出人体手臂与机器人之间的最短距离，从而进行碰撞检测。

4 结论(Conclusion)

本文针对人机共存时的安全问题，提出了一种人与机器人共存中的位姿估计与碰撞检测方法。利用光学3维动作捕捉系统和角度传感器信息，获取了人体手臂的运动信息。通过胶囊体建立了人体手臂和机器人的碰撞包围模型，并实现了碰撞检测。为了验证本文方法的有效性，进行了3组对比实验：(1) 将角度传感器和IMU在同一运动下获取的肘关节角度进行对比；(2) 通过本文方法计算获得的人体前臂末端运动轨迹和光学3维动作捕捉系统测量获得的轨迹进行对比；(3) 通过计算人机最短距离实现人机碰撞检测。实验结果表明本文所提方法可以准确地估计人体手臂末端位姿信息，并计算出人机最短距离，从而解决在非结构化环境下人机碰撞检测问题。

未来，将研究如何把人机位姿状态和最短距离反馈给机器人，并进行人机运动规划，实现人机快速自适应避障。