CN113547519B - 一种基于变维缩放的机械臂逆运动学求解方法 - Google Patents

Abstract

本发明涉及一种基于变维缩放的机械臂逆运动学求解方法，将三维路径转换为二维路径，通过各个臂杆的长度制定路径缩放比例关系，规划好机械臂末端点及各关节中心点的二维路径后，将其转换为三维路径，从而进行逆运动求解，得到各关节角度的变化规律。本发明使用最少的自由度来实现机械臂末端点按照期望路径运动；该方法充分利用冗余机械臂的冗余性能，可实现避障；该方法在逆运动学求解中不需要进行迭代求解，可一次性求出运动学逆解；该方法适用于各种障碍物环境中。

Description

一种基于变维缩放的机械臂逆运动学求解方法

技术领域

本发明涉及机器人技术领域，具体而言，涉及一种基于变维缩放的机械臂逆运动学求解方法。

背景技术

冗余机械臂具有多个自由度，有多种运动模式，可在狭小、复杂以及非结构化的环境中执行任务，具有多目标优化能力，不仅具有极好的环境适应性和极高的避障能力，还可以工作于非结构化环境及多设备环境。在实际应用中，需要进行逆运动学求解来完成机械臂的路径规划与控制，冗余机械臂的逆运动学问题具有更重要的意义。

采用数值法与智能算法进行逆运动学求解的计算过程往往比较复杂，因此可以采用几何法进行简化，降低求解难度。几何法是冗余机械臂逆运动学求解方法中最早出现的一种方法，该方法只是针对冗余机械臂的具体结构进行简化，通过臂杆之间的几何关系建立关节角与机械臂末端点位姿的约束方程，进而通过机械臂末端点的位姿求解出关节角。

用于逆运动学求解的传统几何法往往只规划了机械臂末端点的运动路径，从而无法满足机械臂整体避障的要求。因此，需要一种新型的冗余机械臂逆运动学求解方法，以应对上述问题。

发明内容

本发明解决的技术问题是：针对现有技术的不足，本发明提供了一种基于变维缩放的机械臂逆运动学求解方法，以解决冗余机械臂运动学逆解的不唯一性、复杂性及无法实现避障等问题。

本发明解决技术的方案是：一种基于变维缩放的机械臂逆运动学求解方法，包括如下步骤：

步骤1：根据臂杆长度、平移范围与偏转角度变化范围得出机械臂整体的运动范围，根据机械臂末端点的三维期望路径判断可行性；若不可行，则终止求解；若可行，执行后续步骤；

步骤2：将机械臂末端点的三维期望路径投影至一平面，变维为二维路径；

步骤3：根据各个臂杆的长度得到各个臂杆末端点路径的缩放比例关系，进而根据机械臂末端点的二维路径计算出不考虑障碍物时，其余关节中心点即臂杆末端点在上述平面上投影的二维路径；

步骤4：根据障碍物的位置与大小，调整机械臂末端点在上述平面的二维运动路径；

步骤5：根据上述确定的缩放比例关系结合障碍物的位置，从机械臂末端向起始端依次调整其余关节中心点在上述平面的二维运动路径；

步骤6：将各个关节中心点的二维路径转换为三维路径；

步骤7：根据步骤6转换的三维路径，求得各个关节的俯仰角和偏航角的变化规律，根据该变化规律实现机械臂的控制。

进一步的，步骤2中的平面为机械臂处于水平状态，与机械臂轴线相垂直的平面。

进一步的，步骤3中的缩放比例关系计算公式如下：

式中，k_{i_j}为第i个关节中心点位置与第j个关节中心点位置之间的比例关系，L_w为第w个臂杆两端关节中心点之间的距离。

进一步的，步骤3中关节中心点即臂杆末端点在上述平面上投影的二维路径，以机械臂处于水平状态，机械臂轴线与平面的交点作为缩放中心点，按照缩放比例关系进行缩放。

进一步的，步骤4中的调整通过下述方式实现：

4.1、判断机械臂末端点期望路径上以及期望路径与机械臂末端点的初始位置之间是否有障碍物，若无，则按照机械臂末端点移动距离最短的原则机械臂末端点的初始位置调整至期望运动路径上；否则，转步骤4.2；

4.2、根据每个障碍物确定其等距点，所述的等距点位于期望路径上，从机械臂末端点初始位置开始，在上述平面内两个方向上绕过障碍物到达等距点的距离相等，确定离机械臂末端点初始位置最近的等距点，该等距点记为Pobs1点，对应的障碍物记为obs1

4.3、根据每个障碍物的位置确定最小安全区域，确定从机械臂末端点的初始位置绕过obs1的最小安全区域到达Pobs1点的路径，沿期望路径到达其余障碍物的等距点时，增加机械臂末端点绕对应障碍物的最小安全距离重新回到其等距点的路径。

进一步的，机械臂末端点绕过障碍物到达对应等距点的路径为圆弧、直线或平滑曲线形式。

进一步的，步骤5中通过下述方式调整其余关节中心点在上述平面的二维运动路径：

从机械臂末端点记为第i+1个关节中心点开始执行如下处理：

利用缩放比例关系及第i+1个关节中心点的位置求解第i个关节中心点的位置，如果求解出的点Pi在最小安全区域之外，则把点Pi作为当前的第i个关节中心点位置；否则过点Pi分别做两条直线，与期望路径相交于四个点，从这四个点中选出与第i个关节中心点在上一个时间点时的位置最近的一个点作为当前的第i个关节中心点位置；

依次类推，求解各个关节中心点的二维投影位置，从而调整得到各个关节中心点的二维运动路径。

进一步的，两条直线优选为相互垂直的两条直线，其中一条直线方向与重力方向相同。

进一步的，步骤6中已知第i+1个关节中心点的三维坐标(x_i+1,y_i+1,z_i+1)，通过下述公式可求得第i个关节中心点的三维坐标(x_i,y_i,z_i)中的具体数值，通过下述公式实现二维(y,z)路径转换为三维(x,y,z)路径：

其中，第i+1个关节中心点的三维坐标(x_i+1,y_i+1,z_i+1)、第i个关节中心点的三维坐标(x_i,y_i,z_i)，L_i为第i个臂杆两端关节中心点之间的距离，k_{i_i+1}为第i个关节中心点位置与第i+1个关节中心点位置之间的比例关系。

本发明适用于连续型机械臂、准连续型机械臂、蛇形机械臂、超冗余机械臂、串联机械臂。

本发明与现有技术相比的有益效果是：

本方法将三维路径转换为二维路径，通过各个臂杆的长度制定路径缩放比例关系，规划好机械臂末端点及各关节中心点的二维路径后，将其转换为三维路径，从而进行逆运动求解，得到各关节角度的变化规律。本发明使用最少的自由度来实现机械臂末端点按照期望路径运动；该方法充分利用冗余机械臂的冗余性能，可实现避障；该方法在逆运动学求解中不需要进行迭代求解，可一次性求出运动学逆解；该方法适用于各种障碍物环境中。

附图说明

图1变维缩放法的流程图；

图2冗余机械臂的简化模型及其D-H坐标系；

图3关节角与臂角示意图；

图4所有关节中心点的三维路径与二维投影图；

图5机械臂末端点在第一种障碍物环境中的二维路径；

图6机械臂末端点在第二种障碍物环境中的二维路径；

图7末端点分别位于A4点时的机械臂二维投影图；

图8末端点分别位于A5点时的机械臂二维投影图；

图9求解各个关节中心点的二维投影位置的过程；

图10末端点从A5点运动到A6点时的机械臂二维投影图的变化过程。

具体实施方式

下面结合实施例对本发明作进一步阐述。

本方法首先不考虑障碍物，将机械臂末端点的路径按照一定的比例进行路径缩放，从而得到各个关节的路径，其中的比例关系与臂杆长度相关。如果臂杆沿着该路径运动时会触碰到障碍物，则按照障碍物的位置和一定的比例关系调节机械臂末端点及各个关节中心点的运动路径。规划出即可实现机械臂末端点按照期望路径运动，又实现避障的各个关节中心点的路径。最终通过各个关节中心点的运动路径进行逆运动学求解，可求得各个关节的俯仰角和偏航角的变化规律。

图2是本发明一实施例的冗余机械臂的简化模型及其D-H坐标系的示意图。该机械臂由8个串联臂杆组成，其中每个臂杆具有俯仰θ_i和偏航α_i两个自由度，共16个旋转自由度。每个臂杆由3根绳驱动，每根绳的位置由1个电机驱动，即由24个电机驱动24根绳控制8个臂杆的16个旋转自由度。

为了对绳索驱动冗余机械臂进行运动学分析与求解，需要建立对应的Denavit-Hartenberg(D-H)坐标系。但D-H法作为通用的机器人学求解方法，但其标准并不统一，很多学者根据不同的规则和习惯来建立。传统的D-H坐标系法，采用图2第一行所示的坐标系建立16个自由度的D-H坐标系，需要列出16行D-H坐标系的参数。本发明为了方便计算正、逆运动学，将16坐标系简化为如图2中间一行所示的8个臂杆坐标系，既只需要分析8个臂杆坐标系的D-H参数即可，D-H参数如表1所示。

表1 D-H参数

坐标系{i-1}沿X_i-1轴平移a_i，然后绕Z_i-1轴旋转α_i，再绕Y_i轴旋转θ_i到坐标系{O_i}，最后沿Z_i轴平移d_i，这两个坐标系之间的转换矩阵

可表示为：

式中c代表cos，s代表sin，此后一律采用此约定；

与

分别表示坐标系{i}相对于坐标系{i-1}的旋转变换矩阵与平移变换矩阵；a_i等于L_i-1。

旋转关节可以有一个、两个或三个自由度，本发明中的臂杆之间通过中空万向节连接，每个万向节都可以在俯仰和偏航这两个方向上进行旋转，换句话说就是，第i个万向节具有θ_i和α_i这两个自由度，为了便于分析相邻臂杆之间的位置关系，可将这两个关节角转换为臂形角ψ_i与位置角γ_i。从图3中可以看出关节角与臂角之间的关系，可表示为：

ψ_i＝arccos[cos(θ_i)·cos(α_i)]

通过转换矩阵可以分析出驱动绳长、关节角度、每个臂杆末端点位置之间的映射关系。但如果想通过机械臂末端点的位置(x₉,y₉,z₉)求解各个关节角度和一个整体平移距离，相当于给定3个方程求解17个未知量，一般会对应无数个的解。为了解决超冗余机械臂运动学逆解的不唯一性及复杂性的问题，并实现避障的功能，本发明提出了一种基于变维缩放的机械臂逆运动学求解方法——变维缩放法。

一种基于变维缩放的机械臂逆运动学求解方法，除了上述的建模过程，具体步骤如下：

步骤1：由于单个臂杆存在臂杆长度、平移与偏转角度范围等限制因素，所以单个臂杆只能在一定的范围内运动，进而限制了机械臂整体的运动范围。根据臂杆长度、平移与偏转角度范围得出机械臂整体的运动范围，根据机械臂末端点的三维期望路径判断可行性；若不可行，则终止求解；若可行，执行后续步骤。

步骤2：将机械臂末端点的三维期望路径投影至一平面，变维为二维路径。平面为机械臂处于水平状态，与机械臂轴线相垂直的平面。

由于结构的限制，使得超冗余机械臂在Y₀轴与Z₀轴方向上的运动范围受限，但本发明中的机械臂在X₀轴的方向上具有一个整体平移自由度，所以该机械臂在X₀轴方向上具有很大的运动范围，可以借助该优点，先将三维问题转换为二维问题，降低逆运动学求解的复杂程度。在本步骤中，将机械臂末端点的三维期望路径投影至Y₀Z₀平面，即将规划的三维路径转换为二维路径，为中间关节的路径规划打下基础。

步骤3：缩放末端点的二维路径。根据各个臂杆的长度得到各个臂杆末端点路径的缩放比例关系，进而根据机械臂末端点的二维路径计算出不考虑障碍物时，其余关节中心点即臂杆末端点在上述平面上投影的二维路径。

本步骤暂且不考虑障碍物，以机械臂处于水平状态，机械臂轴线与平面的交点作为缩放中心点，按照缩放比例关系进行缩放机械臂末端点的二维路径，得到各个关节中心点的二维参考运动路径。为后面考虑障碍物时，如何更好的调节各个关节点的二维路径打下参照。

本发明以机械臂末端点沿着1500×1500的正方形运动为例，讲述变维缩放法的过程。机械臂末端点O₉的路径为在

的平面上画出图4中的黑色正方形线框。第一个关节的旋转中心点O₁只能沿着X₀轴运动，故其路径沿着X₀轴的投影为一个点。为了解决运动学逆解的不唯一性及复杂性的问题，令第一个至第八个关节的旋转中心点O₁-O₈的路径沿着X₀轴的投影与机械臂末端点的路径之间存在一定的缩放比例k_{1_9}-k_{8_9}的关系，该比例关系k_{i_9}与臂杆长度L_i相关，可表示为：

从公式中可以看出k_{i_9}的取值在0到1之间，例如k_{1_9}为0，k_{9_9}为1。

上述公式只是分析了第i个关节中心点位置与末端点位置之间的比例关系，进而可以推得第i个关节中心点位置与第j个关节中心点位置之间的比例关系：

通过上述缩放比例的关系，可根据机械臂末端点的路径规划出所有关节点在Y₀Z₀平面上投影的二维路径，根据每个臂杆的长度L_i由末端点至前面关节中心点的方向可以求得所有关节中心点的三维路径，例如已知第i+1个关节中心点O_i+1的三维坐标(x_i+1,y_i+1,z_i+1)，第i个关节中心点O_i的三维坐标(x_i,y_i,z_i)中的未知量x_i，可表示为：

所有关节中心点路径的三维与二维投影图如图4所示。

图中为各个关节中心点运动路径的三视图与斜视图，图中的一个小球代表一个关节中心点经过的离散点。斜视图及侧视图中还包括各个关节中心点运动路径的二维投影。

步骤4：根据障碍物，调整机械臂末端点在上述平面的二维运动路径。

本步骤根据障碍物的位置、形状、大小以及障碍物的初始位置等因素，分为几种不同的方式，将机械臂末端点的期望路径扩展为实际运动路径。

步骤4中的调整通过下述方式实现：

4.1、判断机械臂末端点期望路径上以及期望路径与机械臂末端点的初始位置之间是否有障碍物，若无，则按照机械臂末端点移动距离最短的原则机械臂末端点的初始位置调整至期望运动路径上；否则，转步骤4.2；

4.2、根据每个障碍物确定其等距点，所述的等距点位于期望路径上，从机械臂末端点初始位置开始，在上述平面内两个方向上绕过障碍物到达等距点的距离相等，确定离机械臂末端点初始位置最近的等距点，该等距点记为Pobs1点，对应的障碍物记为obs1

4.3、根据每个障碍物的位置确定最小安全区域，确定从机械臂末端点的初始位置绕过obs1的最小安全区域到达Pobs1点的路径，沿期望路径到达其余障碍物的等距点时，增加机械臂末端点绕对应障碍物的最小安全距离重新回到其等距点的路径。

本发明中以三种障碍物为例，针对两种问题提出了一些具体的解决方案。

针对如何规划路径，在不会碰到障碍物的前提下，使得机械臂末端点从初始位置运动至期望路径上的问题。可以根据许多优化指标或方案进行规划，本发明中机械臂末端点的初始位置不在期望路径上，需要制定相应方案，使得机械臂末端点从初始位置运动至期望路径上。本发明中介绍了“距离最短”、“至等距点”、“至最近等距点”这三种方案。

“距离最短”方案即在期望路径上寻找一个距离机械臂末端点最近的点，然后在不碰到障碍物的前提下，规划从初始位置运动至该最近点的路径，图中从A₁点运动到A₂点即为其中的一个例子。

“至等距点”方案即在期望路径上找到唯一的等距点，然后在不碰到障碍物的前提下，规划从初始位置运动至该最近点的路径。在obs1和obs2中，将机械臂末端点的初始位置A₁点或B₁点与障碍物的质心投影点O_obs1或O_obs2连接并延长，与期望路径的交点即为等距点，等距点是指在与障碍物中心线垂直的投影平面上，从两个方向上绕过障碍物到达该点的距离相等，例如图中的A′₄点与B₂(B₇)点都为等距点。在obs2中找到了等距点，可规划出从初始位置沿着障碍物运动至等距点的路径，也可以规划出其他形式的路径，在本文中在保证不碰到障碍物的前提下，简化了路径的复杂程度，规划出了从B₁点运动至B₂点的障碍物上面的圆弧路径，同理规划出了从期望路径回到初始位置的从B₇点运动至B₈点的障碍物下面的圆弧路径。但有的时候障碍物不仅有一个，并且等距点也不只一个，所以需要“至最近等距点”方案。

“至最近等距点”方案即在多个等距点中找到最近的等距点，然后在不碰到障碍物的前提下，规划从初始位置运动至该最近点的路径。

如果在整个运动过程中，机械臂的所有地方都不会碰到障碍物，则按照最基本的变维缩放法即可规划出每个关节中心点的路径，进而实现超冗余机械臂的逆运动学求解，求得各个自由度的变化规律。

针对机械臂末端点沿着规划的路径运动时，如果会碰到障碍物，应该怎么调整机械臂末端点的运动路径的问题。以变维缩放法中的原则为基础，重新调整末端点的二维路径，为下一步如何调整各个关节的二维路径打下基础，从而即可实现避障又可使得机械臂末端点完成在期望路径上运动的任务。

以本发明中的障碍物为例叙述如何调整末端点的运动路径。为了避免碰撞obs1可以让末端点沿着以下各个二维点运动：A₁-A₂-A₃-A₄-A′₄-A₅-A′₅-A′₄-A₆-A₇-A₈-A₉-A₁₀，其中从A₅点到A′₅点为图中的逆时针方向的圆弧，在该规划的路径中虽然用到了等距点A′₄，但路径较为复杂且需要一些额外计算(比如计算A′₅点的位置等)，为了简化末端点运动路径的复杂程度，将末端点的二维运动路径改为：A₁-A₂-A₃-A₄-A₅-A₆-A₇-A₈-A₉-A₁₀，其中从A₄点到A₅点为图中围着障碍物逆时针方向走一圈，在下一个步骤中会具体叙述冗余机械臂绕过障碍物是否可以运动至A₄点(A₅点)。在obs2中，可以将末端点的二维运动路径规划为：B₁-B₂-B₃-B₄-B₅-B₆-B₇-B₈-B₉-B₁₀，其中从B₁点到B₂点、B₉点到B₁₀点为图中的两段圆弧。为了减小臂形角的偏转范围，故规划出B₇-B₈-B₉这一段的运动路径。针对obs3，其末端点的二维运动路径与无障碍物时候的末端点的二维运动路径相同，二维运动路径在图5中可表示为：A₁-A₂-A₃-A₆-A₇-A₈-A₉-A₁₀。

在机械臂末端点从初始位置运动至期望路径上的过程中(例如从A₁点运动到A₂点)，以及为了避障而不在期望路径上运动的过程中(例如从A₄点到A₅点)，为了不让机械臂末端点在X₀＝3126的平面上留下划痕，在规划好y₉、z₉这两个坐标的基础上，可以适当减小x₉。

在本步骤中，根据障碍物调整了末端点的运动路径。在下个步骤中，调节各个关节中心点的运动路径，从而达到了绕过障碍物的目的。

步骤5：根据上述确定的缩放比例关系结合障碍物的位置，从机械臂末端向起始端依次调整其余关节中心点在上述平面的二维运动路径。

假设末端点O₉及各个关节中心点O₈-O₁的二维投影位置为P₉点、P₈点……P₁点，若已知末端点的位置O₉(x₉,y₉,z₉)，即已知P₉点，则依次根据比例关系及障碍物的位置求出P₈点，再求出P₇点，依次类推，直至求出P₁点的位置，图7和图8即为末端点分别位于A₄点、A₅点时的机械臂二维投影图的一个例子。

在本发明中先在二维投影平面内求出各个关节中心点的二维投影位置，再根据各个臂杆的长度得到每个点的三维位置。下面分别针对不同类型的障碍物，具体讲述如何依次求解各个关节中心点的二维投影位置，进而得到各个关节中心点的二维运动路径。

由于本文中的机械臂只有一个X₀轴方向上的一个平移自由度，所以O₁点的二维投影为位置始终不变的P₁点，以末端点位于图9中的P₉点为例，讲述如何依次求得P₉点-P₂点的位置，这种情况对应着末端点的位置受到障碍物的影响。已知P₉点的位置，连接P₁点与P₉点，利用下式中的比例关系求得P`₈点：

即为图中第一幅图中的中间的P`₈点，可以看出该点不在安全范围之内，为一个不可行的点，过该点分别做一个水平线与竖直线，与期望路径相交于四个点，从这四个点中选出与P₈点上一个时间点时所在的位置最近的一个点作为当前的最终的P₈点。

除了画“十字”找交点外，还可以采用画“斜十字”或者过P₁点画一个半径为

的圆找交点，但如果机械臂沿着利用后两种方法所得的路径运动时，臂形角较大。故在本发明中，选择分别做一个水平线与竖直线的方法调整关节中心点的位置。

在求得第8个关节的投影位置P₈点后，连接P₁点与P₈点，利用下式中的比例关系求得P`₇点：

即为图中第二幅图中的中间的P`₇点，可以看出该点不在安全范围之内，为一个不可行的点，过该点分别做一个水平线与竖直线，与期望路径相交于四个点，从这四个点中选出与P₇点上一个时间点时所在的位置最近的一个点作为当前的最终的P₇点。

依次类推，可求得P₆点、P₅点、P₄点。在求得P₄点后，连接P₁点与P₄点，利用下式中的比例关系求得P`₃点：

从图中最后一幅图中可以看出P`₃点在安全范围之内，可视为最终的P₃点。

图9展示了已知末端点在某个具体的离散点处时，如何依次求解出各个关节中心点的二维投影位置。图10以末端点从A₅点运动到A₆点为例，展示了在运动过程中，如何利用变维缩放法求解各个关节中心点的二维投影位置。

末端点从A₅点运动到A_5-1点的过程中，只有末端点的二维投影位置发生了变化，其余八个关节中心点的位置都未发生改变。末端点从A_5-1点运动到A_5-2点的过程中，除了末端点的二维投影位置发生了变化，第8个关节中心点的投影位置也发生了变化，过了一会，第7个关节中心点的投影位置也发生了变化，但前面6个关节中心点的投影位置一直都未发生变化。末端点从A_5-2点运动到A₆点的过程中，末端点的二维投影位置始终都是沿着一条竖直线运动，其他关节中心点的投影位置也始终都在一条竖直线上。通过分析上述过程，可以发现机械臂在运动过程中，始终符合变维缩放法中的原理。

步骤6：将各个关节中心点的二维路径转换为三维路径。

在本步骤中，已知第i+1个关节中心点的三维坐标(x_i+1,y_i+1,z_i+1)，通过下述公式可求得第i个关节中心点的三维坐标(x_i,y_i,z_i)中的具体数值，通过下述公式实现二维(y,z)路径转换为三维(x,y,z)路径：

其中，L_i为第i个臂杆两端关节中心点之间的距离，k_{i_i+1}为第i个关节中心点位置与第i+1个关节中心点位置之间的比例关系。

到本步骤时，可以规划出末端点及各个关节中心点的三维运动路径，即可实现路径规划。

步骤7：根据步骤6转换的三维路径，利用上述改进的D-H坐标系求得各个关节的俯仰角和偏航角的变化规律，最后再将角度转换为驱动绳的长度，进而实现机械臂的控制。

本发明未详细说明部分属于本领域技术人员的公知常识。

Claims (10)

1.一种基于变维缩放的机械臂逆运动学求解方法，其特征在于包括如下步骤：

步骤1：根据臂杆长度、平移范围与偏转角度变化范围得出机械臂整体的运动范围，根据机械臂末端点的三维期望路径判断可行性；若不可行，则终止求解；若可行，执行后续步骤；

步骤2：将机械臂末端点的三维期望路径投影至一平面，变维为二维路径；

步骤3：根据各个臂杆的长度得到各个臂杆末端点路径的缩放比例关系，进而根据机械臂末端点的二维路径计算出不考虑障碍物时，其余关节中心点即臂杆末端点在上述平面上投影的二维路径；

步骤4：根据障碍物的位置与大小，调整机械臂末端点在上述平面的二维运动路径；

步骤5：根据各个臂杆的长度得到各个臂杆末端点路径的缩放比例关系结合障碍物的位置，从机械臂末端向起始端依次调整其余关节中心点在上述平面的二维运动路径；

步骤6：将各个关节中心点的二维路径转换为三维路径；

步骤7：根据步骤6转换的三维路径，求得各个关节的俯仰角和偏航角的变化规律，根据该变化规律实现机械臂的控制。

2.根据权利要求1所述的方法，其特征在于：步骤2中的平面为机械臂处于水平状态，与机械臂轴线相垂直的平面。

3.根据权利要求1所述的方法，其特征在于：步骤3中的缩放比例关系计算公式如下：

式中，k_{i_j}为第i个关节中心点位置与第j个关节中心点位置之间的比例关系，L_w为第w个臂杆两端关节中心点之间的距离。

4.根据权利要求1所述的方法，其特征在于：步骤3中关节中心点即臂杆末端点在上述平面上投影的二维路径，以机械臂处于水平状态，机械臂轴线与平面的交点作为缩放中心点，按照缩放比例关系进行缩放。

5.根据权利要求4所述的方法，其特征在于：步骤4中的调整通过下述方式实现：

4.1、判断机械臂末端点期望路径上以及期望路径与机械臂末端点的初始位置之间是否有障碍物，若无，则按照机械臂末端点移动距离最短的原则机械臂末端点的初始位置调整至期望运动路径上；否则，转步骤4.2；

4.2、根据每个障碍物确定其等距点，所述的等距点位于期望路径上，从机械臂末端点初始位置开始，在上述平面内两个方向上绕过障碍物到达等距点的距离相等，确定离机械臂末端点初始位置最近的等距点，该等距点记为Pobs1点，对应的障碍物记为obs1；

4.3、根据每个障碍物的位置确定最小安全区域，确定从机械臂末端点的初始位置绕过obs1的最小安全区域到达Pobs1点的路径，沿期望路径到达其余障碍物的等距点时，增加机械臂末端点绕对应障碍物的最小安全距离重新回到其等距点的路径。

6.根据权利要求5所述的方法，其特征在于：机械臂末端点绕过障碍物到达对应等距点的路径为圆弧、直线或平滑曲线形式。

7.根据权利要求5所述的方法，其特征在于：步骤5中通过下述方式调整其余关节中心点在上述平面的二维运动路径：

从机械臂末端点记为第i+1个关节中心点开始执行如下处理：

利用缩放比例关系及第i+1个关节中心点的位置求解第i个关节中心点的位置，如果求解出的点Pi在最小安全区域之外，则把点Pi作为当前的第i个关节中心点位置；否则过点Pi分别做两条直线，与期望路径相交于四个点，从这四个点中选出与第i个关节中心点在上一个时间点时的位置最近的一个点作为当前的第i个关节中心点位置；

依次类推，求解各个关节中心点的二维投影位置，从而调整得到各个关节中心点的二维运动路径。

8.根据权利要求7所述的方法，其特征在于：两条直线为相互垂直的两条直线，其中一条直线方向与重力方向相同。

9.根据权利要求1所述的方法，其特征在于：步骤6中已知第i+1个关节中心点的三维坐标(x_i+1,y_i+1,z_i+1)，通过下述公式可求得第i个关节中心点的三维坐标(x_i,y_i,z_i)中的具体数值，通过下述公式实现二维(y,z)路径转换为三维(x,y,z)路径：

其中，第i+1个关节中心点的三维坐标(x_i+1,y_i+1,z_i+1)、第i个关节中心点的三维坐标(x_i,y_i,z_i)，L_i为第i个臂杆两端关节中心点之间的距离，k_{i_i+1}为第i个关节中心点位置与第i+1个关节中心点位置之间的比例关系。

10.根据权利要求1-9之一所述的方法，其特征在于：适用于连续型机械臂、准连续型机械臂、蛇形机械臂、超冗余机械臂、串联机械臂。

Images