CN105437232A - 一种控制多关节移动机器人避障的方法及装置 - Google Patents

Abstract

本发明提供了一种控制多关节移动机器人避障的方法及装置，该方法包括：在机器人按照预先规划的全局最优路径从起始点向目标点移动的过程中，利用视觉导航和红外避障相互协调控制机器人运动，当视觉***检测到预设距离内有障碍物时，暂停机器人底座的前进，参考预先建立的各个关节的运动学几何模型，自动逐级调节各个关节的运动姿态，在各个关节的可行域内摆动关节，从末端执行器到底座，逐级避开障碍物，通过伺服***控制各个关节的位姿和底盘运动方向，继续控制机器人向目标点移动，直到机器人到达目标点，在红外避障过程中同时调整各个关节和底座的运动姿态，避免后续反复调整，提高了多关节移动机器人的避障效率。

Description

一种控制多关节移动机器人避障的方法及装置

技术领域

本发明涉及机器人技术领域，具体而言，涉及一种控制多关节移动机器人避障的方法及装置。

背景技术

目前，随着社会经济的快速发展及社会智能化的不断推进，随之智能机器人应运而生，智能机器人是能够在道路和野外连续地实时自主运动的机器人，已成为当今科技研究领域的热点，主要目的是研究应用人工智能技术，从而使智能机器人在复杂多变的环境下实现自主推理、规划和控制；另外，随着机器人应用范围的日益扩大和智能化逐步加强，智能机器人逐渐向拟人化的方向发展，因而，多关节移动机器人应用越来越广泛，在复杂多变的环境下多关节移动机器人在行驶过程中随时可能遇到障碍物，因此，需要对机器人进行避障路径的规划使其避开障碍物，进而顺利的到达目的地。

当前，相关技术中提供了一种控制多关节移动机器人避障的方法，该方法包括：首先，对机器人进行全局路径规划，即控制机器人由起始位置运动至终止位置，通过机器人的视觉***采集到运动路径的周围环境及固定障碍物的图像信息，生成整个运动路径的环境图像，根据采集到的环境图像通过最短路径算法确定全局路径；然后，机器人从起始位置点出发，通过在机器人底座上安装红外感应传感器，当红外感应传感器检测到有障碍物时，规划探索机器人的底座避开障碍物在全局点附近运动的可行域，但只考虑了机器人的底座的运动及避障，对于空间上机器人的多维多关节的避障并未进行规划，由于现有的多关节移动机器人的红外传感器和视觉***均安装在机器人的底座上，当机器人遇到障碍物时，只能通过改变底座的运动方向来避障，而且每次运动姿态的调整仅仅保证底座可以避障，无法保证机械臂一定能避障，因而需要底座的运动姿态需要反复调整。

在实现本发明的过程中，发明人发现相关技术中至少存在以下问题：相关技术中当机器人感知到障碍物时，只对底座进行避障处理，导致各个关节在继续移动过程中将与障碍物产生碰撞，从而需要不断调整行驶路径，规划得到的避障路径准确度低，既增加了调整的时间，又降低了避障效率，无法实现使多关节移动机器人快速准确地避开障碍物。

发明内容

有鉴于此，本发明实施例的目的在于提供一种控制多关节移动机器人避障的方法及装置，以避免后续反复调整，提高多关节移动机器人的避障效率。

第一方面，本发明实施例提供了一种控制多关节移动机器人避障的方法，该方法包括：

通过多关节移动机器人的末端执行器上设置的视觉***采集上述机器人所在位置的环境图像，并根据采集到的上述环境图像控制上述机器人按照预先规划的全局最优路径从起始点向目标点的移动；其中，上述机器人的各个关节对应有运动学几何模型，上述运动学几何模型是通过分析上述机器人各个关节的相对运动，采用经典D-H法预先建立的；

在上述机器人移动的过程中，当上述视觉***检测到预设距离内有障碍物时，暂停上述机器人底座的前进；

获取上述运动学几何模型对应的局部坐标系，在上述局部坐标系下，根据上述机器人各个关节对应的转角、相邻关节间间距、杆件长度、杆件扭角，以及检测到上述障碍物的视觉***所在位置调节上述机器人各个关节的转角，直到每个关节均避开上述障碍物；

根据调节后的上述转角确定全局坐标系下的各个关节的运动姿态；

通过上述视觉***采集上述机器人当前位置的环境图像，并根据各个关节的上述运动姿态调整上述机器人向上述目标点移动的全局最优路径，按照调整后的上述全局最优路径继续控制上述机器人向上述目标点移动，直到上述机器人到达上述目标点。

结合第一方面，本发明实施例提供了第一方面的第一种可能的实施方式，其中，上述根据调节后的上述转角确定全局坐标系下的各个关节的运动姿态包括：

确定各个关节对应的运动学几何模型建立的局部坐标系中相邻坐标系之间的齐次变换矩阵公式

$\begin{array}{c}{A}_{i-1}^i={\mathrm{Trans}}_z\left(d_i\right){\mathrm{Rot}}_z\left({\mathrm{\θ}}_i\right){\mathrm{Trans}}_x\left(a_i\right){\mathrm{Rot}}_x\left({\mathrm{\α}}_i\right)\\ =\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& d_i\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{\mathrm{cos\θ}}_i& -{\mathrm{sin\θ}}_i& 0& 0\\ {\mathrm{sin\θ}}_i& {\mathrm{cos\θ}}_i& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& a_i\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& {\mathrm{cos\α}}_i& -{\mathrm{sin\α}}_i& 0\\ 0& {\mathrm{sin\α}}_i& {\mathrm{cos\α}}_i& 0\\ 0& 0& 0& 1\end{array}\right]\\ =\left[\begin{array}{cccc}{\mathrm{cos\θ}}_i& -{\mathrm{cos\α}}_i{\mathrm{sin\θ}}_i& {\mathrm{sin\α}}_i{\mathrm{sin\θ}}_i& a_i{\mathrm{cos\θ}}_i\\ {\mathrm{sin\θ}}_i& {\mathrm{cos\α}}_i{\mathrm{cos\θ}}_i& -{\mathrm{sin\α}}_i{\mathrm{cos\θ}}_i& a_i{\mathrm{sin\θ}}_i\\ 0& {\mathrm{sin\α}}_i& {\mathrm{cos\α}}_i& d_i\\ 0& 0& 0& 1\end{array}\right]\end{array}$

其中，d_i表示关节i与关节i-1间距离，θ_i表示关节i的转角，a_i表示关节i与关节i-1间的杆件长度，α_i表示关节i的杆件扭角，Trans_z(d_i)表示i-1坐标系变换到i坐标系下沿z轴平移d_i距离得到的平移矩阵，Rot_z(θ_i)表示i-1坐标系变换到i坐标系下沿z轴逆时针旋转θ_i角度得到的旋转矩阵，Trans_x(a_i)表示i-1坐标系变换到i坐标系下沿x轴平移a_i距离得到的平移矩阵，Rot_x(α_i)表示i-1坐标系变换到i坐标系下沿x轴逆时针旋转α_i角度得到的旋转矩阵；

根据上述齐次变换矩阵公式和各个关节避开障碍物时对应的转角、相邻关节间间距、杆件长度和杆件扭角计算得到关节i-1坐标系转换至关节i坐标系的矩阵；

根据上述关节i-1坐标系转换至关节i坐标系的矩阵和运动姿态计算公式计算得到在局部坐标系下关节i的运动姿态，其中，i表示关节编号，T_i表示第i个关节在局部坐标系下的运动姿态，表示i-1坐标系变换至i坐标系得到的变换矩阵；

根据在局部坐标系下各个关节的上述运动姿态和计算公式P_i＝P×T_i计算得到全局坐标系下各个关节的运动姿态，其中，P_i表示第i个关节在全局坐标系下的运动姿态，T_i表示第i个关节在局部坐标系下的运动姿态，P表示多关节移动机器人底座在全局坐标系下的运动姿态的矩阵表达式且 $P=\left[\begin{array}{cccc}1& 0& 0& x\\ 0& 1& 0& y\\ 0& 0& 0& 0\\ 0& 0& 1& 1\end{array}\right].$

结合第一方面的第一种可能的实施方式，本发明实施例提供了第一方面的第二种可能的实施方式，其中，上述根据各个关节的上述运动姿态调整上述机器人向上述目标点移动的全局最优路径包括：

根据全局坐标系下各个关节的上述运动姿态确定上述机器人在全局坐标系下的避障运动轨迹；

根据上述避障运动轨迹调整上述机器人向上述目标点移动的全局最优路径。

结合第一方面至第一方面的第二种可能的实施方式中的任一种实施方式，本发明实施例提供了第一方面的第三种可能的实施方式，其中，上述在局部坐标系下，根据上述机器人各个关节对应的转角、相邻关节间间距、杆件长度、杆件扭角，以及检测到上述障碍物的视觉***所在位置调节上述机器人各个关节的转角，直到每个关节均避开上述障碍物包括：

选择上述机器人的末端执行器上的关节作为第一调节关节；

在上述第一调节关节的可行域内根据视觉***建立的障碍物模型调节上述第一调节关节的转角，存储使上述第一调节关节首次避开障碍物所对应的转角；

由上述第一调节关节带动与上述第一调节关节相邻的关节运动，继续调节与上述第一调节关节相邻的关节的转角，存储使与上述第一调节关节相邻的关节首次避开障碍物所对应的转角，依次循环，直到当前需要调节的关节为底座。

第二方面，本发明实施例还提供了一种控制多关节移动机器人避障的装置，该装置包括：

第一控制模块，用于通过多关节移动机器人的末端执行器上设置的视觉***采集上述机器人所在位置的环境图像，并根据采集到的上述环境图像控制上述机器人按照预先规划的全局最优路径从起始点向目标点的移动；其中，上述机器人的各个关节对应有运动学几何模型，上述运动学几何模型是通过分析上述机器人各个关节的相对运动，采用经典D-H法预先建立的；

暂停模块，用于在上述机器人移动的过程中，当上述视觉***检测到预设距离内有障碍物时，暂停上述机器人底座的前进；

调节模块，用于获取上述运动学几何模型对应的局部坐标系，在上述局部坐标系下，根据上述机器人各个关节对应的转角、相邻关节间间距、杆件长度、杆件扭角，以及检测到上述障碍物的视觉***所在位置调节上述机器人各个关节的转角，直到每个关节均避开上述障碍物；

确定模块，用于根据调节后的上述转角确定全局坐标系下的各个关节的运动姿态；

第二控制模块，用于通过上述视觉***采集上述机器人当前位置的环境图像，并根据各个关节的上述运动姿态调整上述机器人向上述目标点移动的全局最优路径，按照调整后的上述全局最优路径继续控制上述机器人向上述目标点移动，直到上述机器人到达上述目标点。

结合第二方面，本发明实施例提供了第二方面的第一种可能的实施方式，其中，上述确定模块包括：

第一确定单元，用于确定各个关节对应的运动学几何模型建立的局部坐标系中相邻坐标系之间的齐次变换矩阵公式

$\begin{array}{c}{A}_{i-1}^i={\mathrm{Trans}}_z\left(d_i\right){\mathrm{Rot}}_z\left({\mathrm{\θ}}_i\right){\mathrm{Trans}}_x\left(a_i\right){\mathrm{Rot}}_x\left({\mathrm{\α}}_i\right)\\ =\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& d_i\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{\mathrm{cos\θ}}_i& -{\mathrm{sin\θ}}_i& 0& 0\\ {\mathrm{sin\θ}}_i& {\mathrm{cos\θ}}_i& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& a_i\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& {\mathrm{cos\α}}_i& -{\mathrm{sin\α}}_i& 0\\ 0& {\mathrm{sin\α}}_i& {\mathrm{cos\α}}_i& 0\\ 0& 0& 0& 1\end{array}\right]\\ =\left[\begin{array}{cccc}{\mathrm{cos\θ}}_i& -{\mathrm{cos\α}}_i{\mathrm{sin\θ}}_i& {\mathrm{sin\α}}_i{\mathrm{sin\θ}}_i& a_i{\mathrm{cos\θ}}_i\\ {\mathrm{sin\θ}}_i& {\mathrm{cos\α}}_i{\mathrm{cos\θ}}_i& -{\mathrm{sin\α}}_i{\mathrm{cos\θ}}_i& a_i{\mathrm{sin\θ}}_i\\ 0& {\mathrm{sin\α}}_i& {\mathrm{cos\α}}_i& d_i\\ 0& 0& 0& 1\end{array}\right]\end{array}$

其中，d_i表示关节i与关节i-1间距离，θ_i表示关节i的转角，a_i表示关节i与关节i-1间的杆件长度，α_i表示关节i的杆件扭角，Trans_z(d_i)表示i-1坐标系变换到i坐标系下沿z轴平移d_i距离得到的平移矩阵，Rot_z(θ_i)表示i-1坐标系变换到i坐标系下沿z轴逆时针旋转θ_i角度得到的旋转矩阵，Trans_x(a_i)表示i-1坐标系变换到i坐标系下沿x轴平移a_i距离得到的平移矩阵，Rot_x(α_i)表示i-1坐标系变换到i坐标系下沿x轴逆时针旋转α_i角度得到的旋转矩阵；

第一计算单元，用于根据上述齐次变换矩阵公式和各个关节避开障碍物时对应的转角、相邻关节间间距、杆件长度和杆件扭角计算得到关节i-1坐标系转换至关节i坐标系的矩阵；

第二计算单元，用于根据上述关节i-1坐标系转换至关节i坐标系的矩阵和运动姿态计算公式计算得到在局部坐标系下关节i的运动姿态，其中，i表示关节编号，T_i表示第i个关节在局部坐标系下的运动姿态，表示i-1坐标系变换至i坐标系得到的变换矩阵；

第三计算单元，用于根据在局部坐标系下各个关节的上述运动姿态和计算公式P_i＝P×T_i计算得到全局坐标系下各个关节的运动姿态，其中，P_i表示第i个关节在全局坐标系下的运动姿态，T_i表示第i个关节在局部坐标系下的运动姿态，P表示多关节移动机器人底座在全局坐标系下的运动姿态的矩阵表达式且 $P=\left[\begin{array}{cccc}1& 0& 0& x\\ 0& 1& 0& y\\ 0& 0& 0& 0\\ 0& 0& 1& 1\end{array}\right].$

结合第二方面的第一种可能的实施方式，本发明实施例提供了第二方面的第二种可能的实施方式，其中，上述第二控制模块包括：

第二确定单元，用于根据全局坐标系下各个关节的上述运动姿态确定上述机器人在全局坐标系下的避障运动轨迹；

调整单元，用于根据上述避障运动轨迹调整上述机器人向上述目标点移动的全局最优路径。

结合第二方面至第二方面的第二种可能的实施方式中的任一种实施方式，本发明实施例提供了第二方面的第三种可能的实施方式，其中，上述调节模块包括：

选取单元，用于选择上述机器人的末端执行器上的关节作为第一调节关节；

第一调节单元，用于在上述第一调节关节的可行域内根据视觉***建立的障碍物模型调节上述第一调节关节的转角，存储使上述第一调节关节首次避开障碍物所对应的转角；

第二调节单元，用于由上述第一调节关节带动与上述第一调节关节相邻的关节运动，继续调节与上述第一调节关节相邻的关节的转角，存储使与上述第一调节关节相邻的关节首次避开障碍物所对应的转角，依次循环，直到当前需要调节的关节为底座。

本发明实施例提供的控制多关节移动机器人避障的方法及装置，该方法包括：在机器人按照预先规划的全局最优路径从起始点向目标点移动的过程中，利用视觉导航和红外避障相互协调控制机器人运动，当视觉***检测到预设距离内有障碍物时，暂停机器人底座的前进，参考预先建立的各个关节的运动学几何模型，自动逐级调节各个关节的运动姿态，在各个关节的可行域内摆动关节，从末端执行器到底座，逐级避开障碍物，通过伺服***控制各个关节的位姿和底盘运动方向，继续控制机器人向目标点移动，直到机器人到达目标点，在红外避障过程中同时调整各个关节和底座的运动姿态，避免后续反复调整，提高了多关节移动机器人的避障效率。

为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1示出了本发明实施例所提供的一种控制多关节移动机器人避障的方法的流程示意图；

图2a示出了本发明实施例所提供的分析多关节移动机器人的各个关节的D-H建模过程中的相邻关节运动学建模示意图；

图2b示出了本发明实施例所提供的分析多关节移动机器人的各个关节的D-H建模过程中的相邻x轴平行，z轴旋转θ角度的示意图；

图2c示出了本发明实施例所提供的分析多关节移动机器人的各个关节的D-H建模过程中的将坐标系沿着z轴平移d距离，使x轴共线的示意图；

图2d示出了本发明实施例所提供的分析多关节移动机器人的各个关节的D-H建模过程中的一种将坐标系沿着x轴平移a距离的示意图；

图2e示出了本发明实施例所提供的分析多关节移动机器人的各个关节的D-H建模过程中的另一种将坐标系沿着x轴平移a距离的示意图；

图2f示出了本发明实施例所提供的分析多关节移动机器人的各个关节的D-H建模过程中的一种将z轴绕着x轴旋转，完成从坐标系x_i-z_i运动到坐标系x_i+1-z_i+1的示意图；

图2g示出了本发明实施例所提供的分析多关节移动机器人的各个关节的D-H建模过程中的另一种将z轴绕着x轴旋转，完成从坐标系x_i-z_i运动到坐标系x_i+1-z_i+1的示意图；

图3示出了本发明实施例所提供的多关节移动机器人的坐标系示意图；

图4示出了本发明实施例所提供的在全局坐标系下，多关节移动机器人从起始点A到目标点B的轨迹的示意图；

图5示出了本发明实施例所提供的多关节移动机器人从起始点A运动到目标点B的最优避障轨迹的示意图；

图6示出了本发明实施例所提供的一种控制多关节移动机器人避障的装置的结构示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

考虑到相关技术中当机器人感知到障碍物时，只对底座进行避障处理，导致各个关节在继续移动过程中将与障碍物产生碰撞，从而需要不断调整行驶路径，规划得到的避障路径准确度低，既增加了调整的时间，又降低了避障效率，无法实现使多关节移动机器人快速准确地避开障碍物。基于此，本发明实施例提供了一种控制多关节移动机器人避障的方法及装置，下面通过实施例进行描述。

如图1所示，本发明实施例提供了一种控制多关节移动机器人避障的方法，该方法包括步骤S102-S110，具体如下：

步骤S102：通过多关节移动机器人的末端执行器上设置的视觉***采集上述机器人所在位置的环境图像，并根据采集到的上述环境图像控制上述机器人按照预先规划的全局最优路径从起始点向目标点的移动；其中，上述机器人的各个关节对应有运动学几何模型，上述运动学几何模型是通过分析上述机器人各个关节的相对运动，采用经典D-H法预先建立的；

步骤S104：在上述机器人移动的过程中，当上述视觉***检测到预设距离内有障碍物时，暂停上述机器人底座的前进；

步骤S106：获取上述运动学几何模型对应的局部坐标系，在上述局部坐标系下，根据上述机器人各个关节对应的转角、相邻关节间间距、杆件长度、杆件扭角，以及检测到上述障碍物的视觉***所在位置调节上述机器人各个关节的转角，直到每个关节均避开上述障碍物；

步骤S108：根据调节后的上述转角确定全局坐标系下的各个关节的运动姿态；

步骤S110：通过上述视觉***采集上述机器人当前位置的环境图像，并根据各个关节的上述运动姿态调整上述机器人向上述目标点移动的全局最优路径，按照调整后的上述全局最优路径继续控制上述机器人向上述目标点移动，直到上述机器人到达上述目标点。

在本发明实施例中，首先，对多关节移动机器人的各个关节建立对应的运动学几何模型，同时保存各个关节在局部坐标系下的对应坐标转角，从规定的起始点到目标点确立全局最优路径；其中，上述运动学几何模型是通过分析机器人各个关节的相对运动，采用经典D-H法建立，上述运动学几何模型为机器人运动空间内封闭几何模型，包括，实体模型，多边形网格模型，栅格模型；上述坐标转角是多关节移动机器人在运动过程中，示教盒上显示的各个关节的转角，通过矩阵变换得出机器人各个关节的运动姿态；上述全局最优路径是通过Bugs算法规划得到的，在全局环境下，是一种可以栅格化的几何表述形式；上述障碍物包括多关节移动机器人运动轨迹上，通过安装在末端执行器上的红外传感器感知的人和物体；

然后，多关节移动机器人从起始点出发，通过机器人的视觉***采集一帧图像，在环境的灰度图像中提取路径，当红外传感器检测到障碍物时，机器人底座停止前进，末端执行器带动相邻关节运动，逐级传递到机器人底座，改变各个关节的姿态，最终引导机器人各个关节均避开障碍物；其中，上述起始点是在全局参考坐标系下，多关节移动机器人在可视化环境下，通过点栅格化表示的特征点；多关节相对运动通过伺服控制***完成；上述视觉***包括安装在机器人末端执行器的视觉传感器，该视觉传感器包括激光雷达、Kinect、Realsense等深度摄像头，上述视觉***还包括安装在机器人末端执行器的红外传感器，该红外传感器主要用于红外测距；上述机器人各个关节的运动姿态通过建立在多关节移动机器人底座的两驱动轮中点处的局部坐标系和底座两驱动轮的中点在全局坐标系下的运动轨迹确立，其中，前者由机器人相邻关节的相对运动和保存在示教盒上关节的转角计算得出，后者以起始点为原点建立的全局参考坐标系下，跟踪机器人绝对路径；

接下来，多关节移动机器人从起始点到目标点在规划的全局最优路径中运动，由视觉导航完成，当感知到预设距离内存在障碍物时，多关节移动机器人进行红外避障，自动调整整个机器人***的运动姿态，机器人各个关节逐级避障，再在视觉导航下向目标点运动；其中，上述视觉导航是通过安装在末端执行器的视觉***来完成的；上述红外避障通过机器人视觉***中的红外传感器发射的红外，当多关节移动机器人的预设距离内有障碍物，反射回视觉***接收器，测出障碍物的距离，与预设的避障阈值进行比较分析以便多关节移动机器人对此及时作出反应，即红外避障是指：当将要遇到障碍物时，机器人底座停止前进，通过安装在机器人末端执行器上的视觉***做出反应，调整多关节移动机器人各个轴的姿态避开障碍物，为了保障整个机械***的可靠性和稳定性以及路径规划更光顺，离末端执行器较近的关节转角变化较大，靠近底座的关节转角变化较小。

本发明实施例给出一种多关节移动机器人避障路径规划方法，同时对多关节移动机器人的底座及各个关节进行运动学分析，首先，利用经典D-H法对多关节移动机器人的各个关节进行运动学建模，建立各个关节的运动学几何模型，在全局参考坐标系下指定机器人运动的起始点和目标点，通过深度测试，基于Bugs算法，获得多关节移动机器人在指定起始点和目标点之间运动的全局最优路径，如果多关节移动机器人通过安装在机器人末端执行器上的视觉***感知到障碍物，考虑机器人各个关节的动力学特征，机械臂在可行域内摆动关节，从末端执行器到底座，逐级避开障碍物，伺服***控制机器人的运动姿态，此过程叫红外避障，同时机器人的运动姿态，可以通过示教盒上的转角，进行相应的矩阵变换来获得。然后，多关节移动机器人继续按照视觉导航朝着目标点移动，直到到达目标点。本发明实施例提供的控制多关节移动机器人避障的方法计算效率高,通过对机器人的D-H建模，可以快速计算出机器人避障时各个关节的运动轨迹，适用于多关节移动机器人对复杂几何模型的障碍物进行自主避障，可以实现高效地为多关节移动机器人规划避障路径，以使多关节移动机器人在复杂的环境中快速地到达目标点，保证机器人自主移动和避障并顺利完成预定任务。

结合上述分析可知，与相关技术相比，本发明实施例在多关节移动机器人避障路径规划中，将视觉***安装在移动机器人的末端执行器，用视觉导航和红外避障相互协调控制机器人的导航，并在其中对任意障碍物的模型，机器人自动逐级调整各个关节的运动姿态，通过伺服***控制移动机器人各个关节的位姿和底盘运动方向，在关节摆动的可行域内优化机器人运动方向，避免了机器人在避障路径规划中的反复调整，减少了机器人避障路径规划的时间，规划出了优化的避障路径，整个过程可以自动完成，计算效率高，编程实现简单，可以应用于室内和室外复杂环境下多关节移动机器人避障路径规划。

进一步的，上述根据调节后的上述转角确定全局坐标系下的各个关节的运动姿态包括：

确定各个关节对应的运动学几何模型建立的局部坐标系中相邻坐标系之间的齐次变换矩阵公式

$\begin{array}{c}{A}_{i-1}^i={\mathrm{Trans}}_z\left(d_i\right){\mathrm{Rot}}_z\left({\mathrm{\θ}}_i\right){\mathrm{Trans}}_x\left(a_i\right){\mathrm{Rot}}_x\left({\mathrm{\α}}_i\right)\\ =\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& d_i\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{\mathrm{cos\θ}}_i& -{\mathrm{sin\θ}}_i& 0& 0\\ {\mathrm{sin\θ}}_i& {\mathrm{cos\θ}}_i& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& a_i\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& {\mathrm{cos\α}}_i& -{\mathrm{sin\α}}_i& 0\\ 0& {\mathrm{sin\α}}_i& {\mathrm{cos\α}}_i& 0\\ 0& 0& 0& 1\end{array}\right]\\ =\left[\begin{array}{cccc}{\mathrm{cos\θ}}_i& -{\mathrm{cos\α}}_i{\mathrm{sin\θ}}_i& {\mathrm{sin\α}}_i{\mathrm{sin\θ}}_i& a_i{\mathrm{cos\θ}}_i\\ {\mathrm{sin\θ}}_i& {\mathrm{cos\α}}_i{\mathrm{cos\θ}}_i& -{\mathrm{sin\α}}_i{\mathrm{cos\θ}}_i& a_i{\mathrm{sin\θ}}_i\\ 0& {\mathrm{sin\α}}_i& {\mathrm{cos\α}}_i& d_i\\ 0& 0& 0& 1\end{array}\right]\end{array}$

其中，d_i表示关节i与关节i-1间距离，θ_i表示关节i的转角，a_i表示关节i与关节i-1间的杆件长度，α_i表示关节i的杆件扭角，Trans_z(d_i)表示i-1坐标系变换到i坐标系下沿z轴平移d_i距离得到的平移矩阵，Rot_z(θ_i)表示i-1坐标系变换到i坐标系下沿z轴逆时针旋转θ_i角度得到的旋转矩阵，Trans_x(a_i)表示i-1坐标系变换到i坐标系下沿x轴平移a_i距离得到的平移矩阵，Rot_x(α_i)表示i-1坐标系变换到i坐标系下沿x轴逆时针旋转α_i角度得到的旋转矩阵；

根据上述齐次变换矩阵公式和各个关节避开障碍物时对应的转角、相邻关节间间距、杆件长度和杆件扭角计算得到关节i-1坐标系转换至关节i坐标系的矩阵；

根据上述关节i-1坐标系转换至关节i坐标系的矩阵和运动姿态计算公式(该运动姿态计算公式也可以表示为)计算得到在局部坐标系下关节i的运动姿态，其中，i表示关节编号，T_i表示第i个关节在局部坐标系下的运动姿态，表示i-1坐标系变换至i坐标系得到的变换矩阵；

将各个关节对应的局部坐标系下的上述运动姿态转换至全局坐标系下的运动姿态。

具体的，如图2a至图2g示出了分析多关节移动机器人的各个关节的D-H建模过程的示意图。如图2a所示，对机器人相邻关节运动学建模，必须分别优先确定z轴，原点，x轴，y轴(可忽略)。如下详细分析了整个坐标系的建立过程：

(1)通常来讲z轴都是每个对应关节，无论是旋转副或者是移动副都是如此，需要特别注意的是关节i处的z轴，其编号为i-1。关节数为i+1的z轴是编号为i。通过这些编号顺序，对所有的关节变量也都比较容易标出。

(2)从建立的z轴中不难发现，相邻z轴可能平行或者延长线相交，更有可能异面，对本发明实施例中利用D-H法建立的z轴，都是第三种情况，因此可以求出他们之间的最短距离，也就是异面直线的公垂线，在坐标空间上，公垂线必定与x轴共线，而且正方向总是指向后者。例如，a_i是z_i-1与z_i之间的公垂线，那么轴的方向是是指向a_i，如图2b所示。

(3)原点的确定，关节z_i与z_i-1之间个公垂线，即相邻的x轴均不在一个平面，如图2b所示。

(4)对于相邻关节平行时，可以把相邻的公垂线共线，这样就大大简化了整个坐标空间模型，减少了对坐标变换矩阵求解带来的较大的计算量。

(5)针对另外一种特殊情况，例如，z_i与z_i-1相交，这样可以两条相交直线确定一个平面，做垂线以后来确认对应的x轴。

机器人的关节和连杆都分别各有两个变量，关节变量θ_i和d_i，连杆参数α_i和a_i，如图2a所示。

接下来，利用运动学的观点对各个坐标轴之间建立相关性，根据刚体的平移与旋转理论，从坐标系x_i-z_i运动到坐标系x_i+1-z_i+1。

(1)为了使相邻的x轴平行，必须使z轴旋转θ_i角度，如图2b所示。

(2)相邻x轴平行以后，并不一定共线，这时将坐标系沿着z轴平移d_i距离，达到x轴共线，如图2c所示。

(3)相邻坐标系的原点重合：将i坐标系沿着x轴平移a_i距离，如图2d与图2e所示。

(4)最后发现两者之间相差一个角度α_i，将z轴绕着x轴旋转即可完成从坐标系x_i-z_i运动到坐标系x_i+1-z_i+1，如图2f与图2g所示。

经过以上分析，建立如下多关节移动机器人D-H模型，如图3所示，其中，4为末端执行器关节坐标系的原点，0为底座基坐标系的原点，1、2、3为相应关节的坐标系的原点，通过建立的连杆坐标系后，可以完全确立以下四个参数：

杆件长度a_i：相邻两轴异面直线最短距离，指向前者为正。

杆件扭角α_i：相邻两关节的扭转共线所需的角度，绕着前者旋转方向为正。

关节距离d_i：相邻两x轴的最短的距离，指向前者为正。

关节转角θ_i：相邻两x轴共面所需要旋转的角度，绕当前关节旋转方向为正，且规定θ_i∈[-π,π)。

最终确立多关节移动机器人各个关节的坐标系，如图3所示，以四关节机器人为例。

经过如上分析，可以得到相邻坐标系之间的齐次变换矩阵即 $\begin{array}{c}{A}_{i-1}^i={\mathrm{Trans}}_z\left(d_i\right){\mathrm{Rot}}_z\left({\mathrm{\θ}}_i\right){\mathrm{Trans}}_x\left(a_i\right){\mathrm{Rot}}_x\left({\mathrm{\α}}_i\right)\\ =\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& d_i\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{\mathrm{cos\θ}}_i& -{\mathrm{sin\θ}}_i& 0& 0\\ {\mathrm{sin\θ}}_i& {\mathrm{cos\θ}}_i& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& a_i\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& {\mathrm{cos\α}}_i& -{\mathrm{sin\α}}_i& 0\\ 0& {\mathrm{sin\α}}_i& {\mathrm{cos\α}}_i& 0\\ 0& 0& 0& 1\end{array}\right]\\ =\left[\begin{array}{cccc}{\mathrm{cos\θ}}_i& -{\mathrm{cos\α}}_i{\mathrm{sin\θ}}_i& {\mathrm{sin\α}}_i{\mathrm{sin\θ}}_i& a_i{\mathrm{cos\θ}}_i\\ {\mathrm{sin\θ}}_i& {\mathrm{cos\α}}_i{\mathrm{cos\θ}}_i& -{\mathrm{sin\α}}_i{\mathrm{cos\θ}}_i& a_i{\mathrm{sin\θ}}_i\\ 0& {\mathrm{sin\α}}_i& {\mathrm{cos\α}}_i& d_i\\ 0& 0& 0& 1\end{array}\right]\end{array}$

以四关节移动机器人为例，根据上述齐次变换矩阵求出各个关节相邻坐标系之间的齐次变换矩阵，如下式：

$A_0^1=\left[\begin{array}{cccc}{\mathrm{cos\θ}}_1& -{\mathrm{cos\α}}_1{\mathrm{sin\θ}}_1& {\mathrm{sin\α}}_1{\mathrm{sin\θ}}_1& a_1{\mathrm{cos\θ}}_1\\ {\mathrm{sin\θ}}_1& {\mathrm{cos\α}}_1{\mathrm{cos\θ}}_1& -{\mathrm{sin\α}}_1{\mathrm{cos\θ}}_1& a_1{\mathrm{sin\θ}}_1\\ 0& {\mathrm{sin\α}}_1& {\mathrm{cos\α}}_1& d_1\\ 0& 0& 0& 1\end{array}\right]$

$A_1^2=\left[\begin{array}{cccc}{\mathrm{cos\θ}}_2& -{\mathrm{cos\α}}_2{\mathrm{sin\θ}}_2& {\mathrm{sin\α}}_2{\mathrm{sin\θ}}_2& a_2{\mathrm{cos\θ}}_2\\ {\mathrm{sin\θ}}_2& {\mathrm{cos\α}}_2{\mathrm{cos\θ}}_2& -{\mathrm{sin\α}}_2{\mathrm{cos\θ}}_2& a_2{\mathrm{sin\θ}}_2\\ 0& {\mathrm{sin\α}}_2& {\mathrm{cos\α}}_2& d_2\\ 0& 0& 0& 1\end{array}\right]$

$A_2^3=\left[\begin{array}{cccc}{\mathrm{cos\θ}}_3& -{\mathrm{cos\α}}_3{\mathrm{sin\θ}}_3& {\mathrm{sin\α}}_3{\mathrm{sin\θ}}_3& a_3{\mathrm{cos\θ}}_3\\ {\mathrm{sin\θ}}_3& {\mathrm{cos\α}}_3{\mathrm{cos\θ}}_3& -{\mathrm{sin\α}}_3{\mathrm{cos\θ}}_3& a_3{\mathrm{sin\θ}}_3\\ 0& {\mathrm{sin\α}}_3& {\mathrm{cos\α}}_3& d_3\\ 0& 0& 0& 1\end{array}\right]$

$A_3^4=\left[\begin{array}{cccc}{\mathrm{cos\θ}}_4& -{\mathrm{cos\α}}_4{\mathrm{sin\θ}}_4& {\mathrm{sin\α}}_4{\mathrm{sin\θ}}_4& a_4{\mathrm{cos\θ}}_4\\ {\mathrm{sin\θ}}_4& {\mathrm{cos\α}}_4{\mathrm{cos\θ}}_4& -{\mathrm{sin\α}}_4{\mathrm{cos\θ}}_4& a_4{\mathrm{sin\θ}}_4\\ 0& {\mathrm{sin\α}}_4& {\mathrm{cos\α}}_4& d_4\\ 0& 0& 0& 1\end{array}\right]$

进而得到该多关节移动机器人在局部坐标系下，关节1，2，3，4的运动姿态分别为：

$T_1=A_0^1;T_2=A_0^1\×{A_1}^2;T_3=A_0^1\×{A_1}^2\×A_2^3;$

$T_4=A_0^1\×A_1^2\×A_2^3\×A_3^4;$

接下来，根据各个关节局部坐标下的运动姿态确定各个关节全局坐标系下的运动姿态，其中，上述将各个关节对应的局部坐标系下的上述运动姿态转换至全局坐标系下的运动姿态包括：

根据局部坐标系下各个关节的上述运动姿态和计算公式P_i＝P×T_i计算得到全局坐标系下各个关节的运动姿态，其中，P_i表示第i个关节在全局坐标系下的运动姿态，T_i表示第i个关节在局部坐标系下的运动姿态，P表示多关节移动机器人底座在全局坐标系下的运动姿态的矩阵表达式且 $P=\left[\begin{array}{cccc}1& 0& 0& x\\ 0& 1& 0& y\\ 0& 0& 0& 0\\ 0& 0& 1& 1\end{array}\right].$

需要说明的是，多关节移动机器人在未知环境的运动，为了便于分析，将机器人底座运动的平面当做水平面，并将各个关节以底座为中心，得到各个关节在全局坐标系下相对于底座的绝对运动姿态，如图4所示，其中，θ为机器人运动方向，在机器人上的x-y局部坐标系，原点在底座，将平面上的点(x,y)，即多关节移动机器人底座所在的坐标位置，在空间的坐标矩阵表达式为 $P=\left[\begin{array}{cccc}1& 0& 0& x\\ 0& 1& 0& y\\ 0& 0& 0& 0\\ 0& 0& 1& 1\end{array}\right],$ 该矩阵的表达式前三列分别为沿着x，y，z轴的方向，由于z轴垂直于平面，故第3列最后的因子为1。

仍以四关节移动机器人为例，在全局坐标系下，机器人关节1，2，3，4的的绝对运动姿态为：

P₁＝P×T₁，P₂＝P×T₂，P₃＝P×T₃，

P₄＝P×T₄，

其中，矩阵P₁，P₂，P₃，P₄均为4x4矩阵，矩阵的前三列表示机器人各个关节的运动方向，最后一列分别表示在对应x，y，z轴上面的映射值，因此，确定了机器人各个关节的位置和姿态。

在本发明实施例中，基于Bugs算法规划多关节移动机器人的全局最优路径，生成机器人避障路径，得到运动轨迹可行域；如图4所示，在全局坐标系下，从起始点A运动到目标点B的全局路径规划，机器人的运动坐标及方向可以由(x，y，θ_i)确定，机器人各个关节的位姿可以通过局部坐标系和全局坐标系之间的矩阵转换确立。

其中，上述根据各个关节的上述运动姿态调整上述机器人向上述目标点移动的全局最优路径包括：

根据全局坐标系下各个关节的上述运动姿态确定上述机器人在全局坐标系下的避障运动轨迹；

根据上述避障运动轨迹调整上述机器人向上述目标点移动的全局最优路径。

如图5所示，过程a、c、e为视觉导航，过程b、d、f为红外避障，障碍物轮廓为基于Bugs算法的机器人通过视觉***扫描到的障碍物深度信息。

具体的，上述在局部坐标系下，根据上述机器人各个关节对应的转角、相邻关节间间距、杆件长度、杆件扭角，以及检测到上述障碍物的视觉***所在位置调节上述机器人各个关节的转角，直到每个关节均避开上述障碍物包括：

选择上述机器人的末端执行器上的关节作为第一调节关节；

在上述第一调节关节的可行域内根据视觉***建立的障碍物模型调节上述第一调节关节的转角，存储使上述第一调节关节首次避开障碍物所对应的转角；

由上述第一调节关节带动与上述第一调节关节相邻的关节运动，继续调节与上述第一调节关节相邻的关节的转角，存储使与上述第一调节关节相邻的关节首次避开障碍物所对应的转角，依次循环，直到当前需要调节的关节为底座。

本发明实施例提供的控制多关节移动机器人避障的方法，在机器人按照预先规划的全局最优路径从起始点向目标点移动的过程中，利用视觉导航和红外避障相互协调控制机器人运动，当视觉***检测到预设距离内有障碍物时，暂停机器人底座的前进，参考预先建立的各个关节的运动学几何模型，自动逐级调节各个关节的运动姿态，在各个关节的可行域内摆动关节，从末端执行器到底座，逐级避开障碍物，通过伺服***控制各个关节的位姿和底盘运动方向，继续控制机器人向目标点移动，直到机器人到达目标点，在红外避障过程中同时调整各个关节和底座的运动姿态，避免后续反复调整，提高了多关节移动机器人的避障效率。

如图6所示，本发明实施例还提供了一种控制多关节移动机器人避障的装置，该装置包括：

第一控制模块602，用于通过多关节移动机器人的末端执行器上设置的视觉***采集上述机器人所在位置的环境图像，并根据采集到的上述环境图像控制上述机器人按照预先规划的全局最优路径从起始点向目标点的移动；其中，上述机器人的各个关节对应有运动学几何模型，上述运动学几何模型是通过分析上述机器人各个关节的相对运动，采用经典D-H法预先建立的；

暂停模块604，用于在上述机器人移动的过程中，当上述视觉***检测到预设距离内有障碍物时，暂停上述机器人底座的前进；

调节模块606，用于获取上述运动学几何模型对应的局部坐标系，在上述局部坐标系下，根据上述机器人各个关节对应的转角、相邻关节间间距、杆件长度、杆件扭角，以及检测到上述障碍物的视觉***所在位置调节上述机器人各个关节的转角，直到每个关节均避开上述障碍物；

确定模块608，用于根据调节后的上述转角确定全局坐标系下的各个关节的运动姿态；

第二控制模块610，用于通过上述视觉***采集上述机器人当前位置的环境图像，并根据各个关节的上述运动姿态调整上述机器人向上述目标点移动的全局最优路径，按照调整后的上述全局最优路径继续控制上述机器人向上述目标点移动，直到上述机器人到达上述目标点。

进一步的，上述确定模块608包括：

第一确定单元，用于确定各个关节对应的运动学几何模型建立的局部坐标系中相邻坐标系之间的齐次变换矩阵公式

$\begin{array}{c}{A}_{i-1}^i={\mathrm{Trans}}_z\left(d_i\right){\mathrm{Rot}}_z\left({\mathrm{\θ}}_i\right){\mathrm{Trans}}_x\left(a_i\right){\mathrm{Rot}}_x\left({\mathrm{\α}}_i\right)\\ =\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& d_i\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{\mathrm{cos\θ}}_i& -{\mathrm{sin\θ}}_i& 0& 0\\ {\mathrm{sin\θ}}_i& {\mathrm{cos\θ}}_i& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& a_i\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& {\mathrm{cos\α}}_i& -{\mathrm{sin\α}}_i& 0\\ 0& {\mathrm{sin\α}}_i& {\mathrm{cos\α}}_i& 0\\ 0& 0& 0& 1\end{array}\right]\\ =\left[\begin{array}{cccc}{\mathrm{cos\θ}}_i& -{\mathrm{cos\α}}_i{\mathrm{sin\θ}}_i& {\mathrm{sin\α}}_i{\mathrm{sin\θ}}_i& a_i{\mathrm{cos\θ}}_i\\ {\mathrm{sin\θ}}_i& {\mathrm{cos\α}}_i{\mathrm{cos\θ}}_i& -{\mathrm{sin\α}}_i{\mathrm{cos\θ}}_i& a_i{\mathrm{sin\θ}}_i\\ 0& {\mathrm{sin\α}}_i& {\mathrm{cos\α}}_i& d_i\\ 0& 0& 0& 1\end{array}\right]\end{array}$

其中，d_i表示关节i与关节i-1间距离，θ_i表示关节i的转角，a_i表示关节i与关节i-1间的杆件长度，α_i表示关节i的杆件扭角，Trans_z(d_i)表示i-1坐标系变换到i坐标系下沿z轴平移d_i距离得到的平移矩阵，Rot_z(θ_i)表示i-1坐标系变换到i坐标系下沿z轴逆时针旋转θ_i角度得到的旋转矩阵，Trans_x(a_i)表示i-1坐标系变换到i坐标系下沿x轴平移a_i距离得到的平移矩阵，Rot_x(α_i)表示i-1坐标系变换到i坐标系下沿x轴逆时针旋转α_i角度得到的旋转矩阵；

第一计算单元，用于根据上述齐次变换矩阵公式和各个关节避开障碍物时对应的转角、相邻关节间间距、杆件长度和杆件扭角计算得到关节i-1坐标系转换至关节i坐标系的矩阵；

第二计算单元，用于根据上述关节i-1坐标系转换至关节i坐标系的矩阵和运动姿态计算公式计算得到在局部坐标系下关节i的运动姿态，其中，i表示关节编号，T_i表示第i个关节在局部坐标系下的运动姿态，表示i-1坐标系变换至i坐标系得到的变换矩阵；

第三计算单元，用于根据在局部坐标系下各个关节的上述运动姿态和计算公式P_i＝P×T_i计算得到全局坐标系下各个关节的运动姿态，其中，P_i表示第i个关节在全局坐标系下的运动姿态，T_i表示第i个关节在局部坐标系下的运动姿态，P表示多关节移动机器人底座在全局坐标系下的运动姿态的矩阵表达式且 $P=\left[\begin{array}{cccc}1& 0& 0& x\\ 0& 1& 0& y\\ 0& 0& 0& 0\\ 0& 0& 1& 1\end{array}\right].$

进一步的，上述第二控制模块610包括：

第二确定单元，用于根据全局坐标系下各个关节的上述运动姿态确定上述机器人在全局坐标系下的避障运动轨迹；

调整单元，用于根据上述避障运动轨迹调整上述机器人向上述目标点移动的全局最优路径。

进一步的，上述调节模块606包括：

选取单元，用于选择上述机器人的末端执行器上的关节作为第一调节关节；

第一调节单元，用于在上述第一调节关节的可行域内根据视觉***建立的障碍物模型调节上述第一调节关节的转角，存储使上述第一调节关节首次避开障碍物所对应的转角；

第二调节单元，用于由上述第一调节关节带动与上述第一调节关节相邻的关节运动，继续调节与上述第一调节关节相邻的关节的转角，存储使与上述第一调节关节相邻的关节首次避开障碍物所对应的转角，依次循环，直到当前需要调节的关节为底座。

基于上述分析可知，本发明实施例提供的控制多关节移动机器人避障的装置，在机器人按照预先规划的全局最优路径从起始点向目标点移动的过程中，利用视觉导航和红外避障相互协调控制机器人运动，当视觉***检测到预设距离内有障碍物时，暂停机器人底座的前进，参考预先建立的各个关节的运动学几何模型，自动逐级调节各个关节的运动姿态，在各个关节的可行域内摆动关节，从末端执行器到底座，逐级避开障碍物，通过伺服***控制各个关节的位姿和底盘运动方向，继续控制机器人向目标点移动，直到机器人到达目标点，在红外避障过程中同时调整各个关节和底座的运动姿态，避免后续反复调整，提高了多关节移动机器人的避障效率。

本发明实施例所提供的控制多关节移动机器人避障的装置可以为设备上的特定硬件或者安装于设备上的软件或固件等。本发明实施例所提供的装置，其实现原理及产生的技术效果和前述方法实施例相同，为简要描述，装置实施例部分未提及之处，可参考前述方法实施例中相应内容。所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，前述描述的***、装置和单元的具体工作过程，均可以参考上述方法实施例中的对应过程，在此不再赘述。

所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，RandomAccessMemory)、磁碟或者光盘等各种可以存储程序代码的介质。

应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释，此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

最后应说明的是：以上所述实施例，仅为本发明的具体实施方式，用以说明本发明的技术方案，而非对其限制，本发明的保护范围并不局限于此，尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围。都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应所述以权利要求的保护范围为准。

Claims (8)

1.一种控制多关节移动机器人避障的方法，其特征在于，所述方法包括：

通过多关节移动机器人的末端执行器上设置的视觉***采集所述机器人所在位置的环境图像，并根据采集到的所述环境图像控制所述机器人按照预先规划的全局最优路径从起始点向目标点的移动；其中，所述机器人的各个关节对应有运动学几何模型，所述运动学几何模型是通过分析所述机器人各个关节的相对运动，采用经典D-H法预先建立的；

在所述机器人移动的过程中，当所述视觉***检测到预设距离内有障碍物时，暂停所述机器人底座的前进；

获取所述运动学几何模型对应的局部坐标系，在所述局部坐标系下，根据所述机器人各个关节对应的转角、相邻关节间间距、杆件长度、杆件扭角，以及检测到所述障碍物的视觉***所在位置调节所述机器人各个关节的转角，直到每个关节均避开所述障碍物；

根据调节后的所述转角确定全局坐标系下的各个关节的运动姿态；

通过所述视觉***采集所述机器人当前位置的环境图像，并根据各个关节的所述运动姿态调整所述机器人向所述目标点移动的全局最优路径，按照调整后的所述全局最优路径继续控制所述机器人向所述目标点移动，直到所述机器人到达所述目标点。

2.根据权利要求1所述的控制多关节移动机器人避障的方法，其特征在于，所述根据调节后的所述转角确定全局坐标系下的各个关节的运动姿态包括：

确定各个关节对应的运动学几何模型建立的局部坐标系中相邻坐标系之间的齐次变换矩阵公式

$\begin{array}{c}{A}_{i-1}^i={\mathrm{Trans}}_z\left(d_i\right){\mathrm{Rot}}_z\left({\mathrm{\θ}}_i\right){\mathrm{Trans}}_x\left(a_i\right){\mathrm{Rot}}_x\left({\mathrm{\α}}_i\right)\\ =\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& d_i\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{\mathrm{cos\θ}}_i& -{\mathrm{sin\θ}}_i& 0& 0\\ {\mathrm{sin\θ}}_i& {\mathrm{cos\θ}}_i& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& a_i\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& {\mathrm{cos\α}}_i& -{\mathrm{sin\α}}_i& 0\\ 0& {\mathrm{sin\α}}_i& {\mathrm{cos\α}}_i& 0\\ 0& 0& 0& 1\end{array}\right]\\ =\left[\begin{array}{cccc}{\mathrm{cos\θ}}_i& -{\mathrm{cos\α}}_i{\mathrm{sin\θ}}_i& {\mathrm{sin\α}}_i{\mathrm{sin\θ}}_i& a_i{\mathrm{cos\θ}}_i\\ {\mathrm{sin\θ}}_i& {\mathrm{cos\α}}_i{\mathrm{cos\θ}}_i& -{\mathrm{sin\α}}_i{\mathrm{cos\θ}}_i& a_i{\mathrm{sin\θ}}_i\\ 0& {\mathrm{sin\α}}_i& {\mathrm{cos\α}}_i& d_i\\ 0& 0& 0& 1\end{array}\right]\end{array}$

其中，d_i表示关节i与关节i-1间距离，θ_i表示关节i的转角，a_i表示关节i与关节i-1间的杆件长度，α_i表示关节i的杆件扭角，Trans_z(d_i)表示i-1坐标系变换到i坐标系下沿z轴平移d_i距离得到的平移矩阵，Rot_z(θ_i)表示i-1坐标系变换到i坐标系下沿z轴逆时针旋转θ_i角度得到的旋转矩阵，Trans_x(a_i)表示i-1坐标系变换到i坐标系下沿x轴平移a_i距离得到的平移矩阵，Rot_x(α_i)表示i-1坐标系变换到i坐标系下沿x轴逆时针旋转α_i角度得到的旋转矩阵；

根据所述齐次变换矩阵公式和各个关节避开障碍物时对应的转角、相邻关节间间距、杆件长度和杆件扭角计算得到关节i-1坐标系转换至关节i坐标系的矩阵；

根据所述关节i-1坐标系转换至关节i坐标系的矩阵和运动姿态计算公式计算得到在局部坐标系下关节i的运动姿态，其中，i表示关节编号，T_i表示第i个关节在局部坐标系下的运动姿态，表示i-1坐标系变换至i坐标系得到的变换矩阵；

根据在局部坐标系下各个关节的所述运动姿态和计算公式P_i＝P×T_i计算得到全局坐标系下各个关节的运动姿态，其中，P_i表示第i个关节在全局坐标系下的运动姿态，T_i表示第i个关节在局部坐标系下的运动姿态，P表示多关节移动机器人底座在全局坐标系下的运动姿态的矩阵表达式且 $P=\left[\begin{array}{cccc}1& 0& 0& x\\ 0& 1& 0& y\\ 0& 0& 0& 0\\ 0& 0& 1& 1\end{array}\right].$

3.根据权利要求2所述的控制多关节移动机器人避障的方法，其特征在于，所述根据各个关节的所述运动姿态调整所述机器人向所述目标点移动的全局最优路径包括：

根据全局坐标系下各个关节的所述运动姿态确定所述机器人在全局坐标系下的避障运动轨迹；

根据所述避障运动轨迹调整所述机器人向所述目标点移动的全局最优路径。

4.根据权利要求1-3任一项所述的控制多关节移动机器人避障的方法，其特征在于，所述在局部坐标系下，根据所述机器人各个关节对应的转角、相邻关节间间距、杆件长度、杆件扭角，以及检测到所述障碍物的视觉***所在位置调节所述机器人各个关节的转角，直到每个关节均避开所述障碍物包括：

选择所述机器人的末端执行器上的关节作为第一调节关节；

在所述第一调节关节的可行域内根据视觉***建立的障碍物模型调节所述第一调节关节的转角，存储使所述第一调节关节首次避开障碍物所对应的转角；

由所述第一调节关节带动与所述第一调节关节相邻的关节运动，继续调节与所述第一调节关节相邻的关节的转角，存储使与所述第一调节关节相邻的关节首次避开障碍物所对应的转角，依次循环，直到当前需要调节的关节为底座。

5.一种控制多关节移动机器人避障的装置，其特征在于，所述装置包括：

第一控制模块，用于通过多关节移动机器人的末端执行器上设置的视觉***采集所述机器人所在位置的环境图像，并根据采集到的所述环境图像控制所述机器人按照预先规划的全局最优路径从起始点向目标点的移动；其中，所述机器人的各个关节对应有运动学几何模型，所述运动学几何模型是通过分析所述机器人各个关节的相对运动，采用经典D-H法预先建立的；

暂停模块，用于在所述机器人移动的过程中，当所述视觉***检测到预设距离内有障碍物时，暂停所述机器人底座的前进；

调节模块，用于获取所述运动学几何模型对应的局部坐标系，在所述局部坐标系下，根据所述机器人各个关节对应的转角、相邻关节间间距、杆件长度、杆件扭角，以及检测到所述障碍物的视觉***所在位置调节所述机器人各个关节的转角，直到每个关节均避开所述障碍物；

确定模块，用于根据调节后的所述转角确定全局坐标系下的各个关节的运动姿态；

第二控制模块，用于通过所述视觉***采集所述机器人当前位置的环境图像，并根据各个关节的所述运动姿态调整所述机器人向所述目标点移动的全局最优路径，按照调整后的所述全局最优路径继续控制所述机器人向所述目标点移动，直到所述机器人到达所述目标点。

6.根据权利要求5所述的控制多关节移动机器人避障的装置，其特征在于，所述确定模块包括：

第一确定单元，用于确定各个关节对应的运动学几何模型建立的局部坐标系中相邻坐标系之间的齐次变换矩阵公式

$\begin{array}{c}{A}_{i-1}^i={\mathrm{Trans}}_z\left(d_i\right){\mathrm{Rot}}_z\left({\mathrm{\θ}}_i\right){\mathrm{Trans}}_x\left(a_i\right){\mathrm{Rot}}_x\left({\mathrm{\α}}_i\right)\\ =\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& d_i\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}{\mathrm{cos\θ}}_i& -{\mathrm{sin\θ}}_i& 0& 0\\ {\mathrm{sin\θ}}_i& {\mathrm{cos\θ}}_i& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& a_i\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& {\mathrm{cos\α}}_i& -{\mathrm{sin\α}}_i& 0\\ 0& {\mathrm{sin\α}}_i& {\mathrm{cos\α}}_i& 0\\ 0& 0& 0& 1\end{array}\right]\\ =\left[\begin{array}{cccc}{\mathrm{cos\θ}}_i& -{\mathrm{cos\α}}_i{\mathrm{sin\θ}}_i& {\mathrm{sin\α}}_i{\mathrm{sin\θ}}_i& a_i{\mathrm{cos\θ}}_i\\ {\mathrm{sin\θ}}_i& {\mathrm{cos\α}}_i{\mathrm{cos\θ}}_i& -{\mathrm{sin\α}}_i{\mathrm{cos\θ}}_i& a_i{\mathrm{sin\θ}}_i\\ 0& {\mathrm{sin\α}}_i& {\mathrm{cos\α}}_i& d_i\\ 0& 0& 0& 1\end{array}\right]\end{array}$

其中，d_i表示关节i与关节i-1间距离，θ_i表示关节i的转角，a_i表示关节i与关节i-1间的杆件长度，α_i表示关节i的杆件扭角，Trans_z(d_i)表示i-1坐标系变换到i坐标系下沿z轴平移d_i距离得到的平移矩阵，Rot_z(θ_i)表示i-1坐标系变换到i坐标系下沿z轴逆时针旋转θ_i角度得到的旋转矩阵，Trans_x(a_i)表示i-1坐标系变换到i坐标系下沿x轴平移a_i距离得到的平移矩阵，Rot_x(α_i)表示i-1坐标系变换到i坐标系下沿x轴逆时针旋转α_i角度得到的旋转矩阵；

第一计算单元，用于根据所述齐次变换矩阵公式和各个关节避开障碍物时对应的转角、相邻关节间间距、杆件长度和杆件扭角计算得到关节i-1坐标系转换至关节i坐标系的矩阵；

第二计算单元，用于根据所述关节i-1坐标系转换至关节i坐标系的矩阵和运动姿态计算公式计算得到在局部坐标系下关节i的运动姿态，其中，i表示关节编号，T_i表示第i个关节在局部坐标系下的运动姿态，表示i-1坐标系变换至i坐标系得到的变换矩阵；

第三计算单元，用于根据在局部坐标系下各个关节的所述运动姿态和计算公式P_i＝P×T_i计算得到全局坐标系下各个关节的运动姿态，其中，P_i表示第i个关节在全局坐标系下的运动姿态，T_i表示第i个关节在局部坐标系下的运动姿态，P表示多关节移动机器人底座在全局坐标系下的运动姿态的矩阵表达式且 $P=\left[\begin{array}{cccc}1& 0& 0& x\\ 0& 1& 0& y\\ 0& 0& 0& 0\\ 0& 0& 1& 1\end{array}\right].$

7.根据权利要求6所述的控制多关节移动机器人避障的装置，其特征在于，所述第二控制模块包括：

第二确定单元，用于根据全局坐标系下各个关节的所述运动姿态确定所述机器人在全局坐标系下的避障运动轨迹；

调整单元，用于根据所述避障运动轨迹调整所述机器人向所述目标点移动的全局最优路径。

8.根据权利要求5-7任一项所述的控制多关节移动机器人避障的装置，其特征在于，所述调节模块包括：

选取单元，用于选择所述机器人的末端执行器上的关节作为第一调节关节；

第一调节单元，用于在所述第一调节关节的可行域内根据视觉***建立的障碍物模型调节所述第一调节关节的转角，存储使所述第一调节关节首次避开障碍物所对应的转角；

第二调节单元，用于由所述第一调节关节带动与所述第一调节关节相邻的关节运动，继续调节与所述第一调节关节相邻的关节的转角，存储使与所述第一调节关节相邻的关节首次避开障碍物所对应的转角，依次循环，直到当前需要调节的关节为底座。