基于弹性包络的蛇形机器人安全攀爬控制方法与流程

本发明涉及机器人控制领域，特别是涉及基于弹性包络的蛇形机器人安全攀爬控制方法。
背景技术：
：蛇形机器人属于新型仿生机器人，拥有高冗余的自由度，与生物界蛇类一样具有运动敏捷灵活等优点，能克服复杂环境的干扰阻碍，可实现行波、蜿蜒、地面翻滚、螺旋攀爬等多种步态(步态也称运动方式)，出色地完成其他机器人难以胜任的任务，具有广泛应用前景。蛇形机器人具有多关节特点，这一特点虽提升了蛇体运动灵活性，但须通过复杂关节协调实现蛇体步态，解决这一复杂协调问题的直接方法是设计有效的参数化步态。目前，参数化步态设计方法主要有三种：第一种是控制函数步态生成法，即将步态设计成具体关节转角的控制函数方法；第二种是骨干曲线步态生成法，即将蛇体的步态看作是骨干曲线的定向移动方法；第三种是：基于中枢神经发生器(CPG)的蛇形机器人步态控制方法，即模仿生物的步态产生原理为基础的方法。第一种方法中，控制函数形式比较复杂，控制函数中的参数与螺旋攀爬运动的整体形态参数(如螺旋角、螺距等)关系难于对应和量化，而且目前的控制函数法无法实现对螺旋攀爬步态中控制函数的参数优化。第二种方法中，指向性较强，基本上只可应用在万向节关节的蛇形机器人上，例如不适应正交关节蛇形机器人螺旋攀爬运动，通用性较差。第三种方法，不需要直接对蛇形机器人复杂位姿建模，具有明显的优势，也是未来重要研究方向，但目前存在缺少成熟的CPG数学模型，方案也不成熟。总的来说，目前对蛇形机器人的控制方法存在实现方式复杂、通用性差或者方案不成熟等缺陷。技术实现要素：为了解决上述的技术问题，本发明的目的是提供基于弹性包络的蛇形机器人安全攀爬控制方法。本发明解决其技术问题所采用的技术方案是：基于弹性包络的蛇形机器人安全攀爬控制方法，包括步骤：S1、依次控制蛇形机器人在不同控制参数下进行螺旋攀爬运动；S2、采集蛇形机器人在每次螺旋攀爬过程中的位置和姿态，进而求解蛇形机器人的各关节的实时位姿；S3、根据求解获得的蛇形机器人的各关节位姿，计算出各关节与杆件柱心直线的间隙；S4、基于间隙二值图像获得不同时刻下蛇形机器人与杆件的弹性包络规律；S5、计算获得不同弹性包络规律下的攀爬系数，并计算不同控制参数下整个攀爬运动的平均攀爬系数；S6、获取所计算的多个平均攀爬系数的中间值所对应的控制参数，作为蛇形机器人在对应杆件上的安全攀爬控制参数；S7、根据该安全攀爬控制参数控制蛇形机器人在对应杆件上进行攀爬运动。进一步，所述步骤S2，具体包括：S21、采用测距仪器采集蛇形机器人上的特定点到其所攀爬杆件的距离，同时采用陀螺仪采集蛇形机器人的蛇头关节模块的三维方向；S22、分别将测距仪器采集的距离和陀螺仪采集的三维方向作为蛇形机器人的位置和姿态，基于D-H分析法，求解获得蛇形机器人的各关节的实时位姿轨迹。进一步，所述步骤S22，具体包括：S221、分别将测距仪器采集的距离和陀螺仪采集的三维方向作为蛇形机器人的位置和姿态，应用D-H坐标法建模，由空间约束方程得到N个独立空间约束方程，其中，N表示蛇形机器人的关节数量；S222、限定沿杆件轴线方向为蛇形机器人整体运动方向，消去一个自由度；S223、基于陀螺仪采集的三维方向补充3个约束方程；S224、基于测距仪器采集的距离补充1个约束方程；S225、通过以上N+4个约束方程，求解获得蛇形机器人的螺旋攀爬位姿，获得各关节的实时位姿轨迹。进一步，所述步骤S3，其具体为：结合下式，根据求解获得的蛇形机器人的各关节位姿，计算出各关节与杆件柱心直线的间隙：且杆件柱心直线的方程为：上式中，Rli表示蛇形机器人的第i个关节到杆件柱心直线的间隙，(xi,yi,zi)和(xi+1,yi+1,zi+1)分别表示蛇形机器人的第i个关节的两关节点的坐标，(x0,y0,z0)表示杆件柱心的坐标，a、b和c均为常数。进一步，所述步骤S4，具体包括：S41、将时间轴等分离散化，以离散时间点记录各关节与杆件柱心直线的间隙；S42、将间隙为0对应包络并标示为0，间隙大于0对应非包络并标示为1，进而将蛇形机器人的各关节的距离规律生成间隙二值图像；所述间隙二值图像中，像素横坐标从左到右对应表示从蛇头到蛇尾模块，像素纵坐标从下到上对应表示由小到大离散时刻，对每一时刻各关节的间隙为0/非0对应该时刻到下一时刻的像素点标为0/1；S43、根据生成的间隙二值图像获得不同时刻下蛇形机器人与杆件的弹性包络规律。进一步，所述步骤S5，具体包括：S51、根据下式，计算蛇形机器人的各关节的弹性形变量：△ri＝Rli-ry-rg其中，△ri表示蛇形机器人第i个关节的弹性形变量，Rli表示蛇形机器人的第i个关节到杆件柱心直线的间隙，rg表示杆件的半径，ry表示关节块件的半径；S52、根据下式，分别计算蛇形机器人的抱紧力Fn和攀爬力f：上式中，k表示关节块件表面的弹性系数，u表示静摩擦系数，S53、根据下式，分别计算每个弹性包络规律下的攀爬系数λ：上式中，Gi表示第i个关节模块的重力。本发明的有益效果是：基于弹性包络的蛇形机器人安全攀爬控制方法，包括步骤：S1、依次控制蛇形机器人在不同控制参数下进行螺旋攀爬运动；S2、采集蛇形机器人在每次螺旋攀爬过程中的位置和姿态，进而求解蛇形机器人的各关节的实时位姿；S3、根据求解获得的蛇形机器人的各关节位姿，计算出各关节与杆件柱心直线的间隙；S4、基于间隙二值图像获得不同时刻下蛇形机器人与杆件的弹性包络规律；S5、计算获得不同弹性包络规律下的攀爬系数，并计算不同控制参数下整个攀爬运动的平均攀爬系数；S6、获取所计算的多个平均攀爬系数的中间值所对应的控制参数，作为蛇形机器人在对应杆件上的安全攀爬控制参数；S7、根据该安全攀爬控制参数控制蛇形机器人在对应杆件上进行攀爬运动。本方法可以实现攀爬力最优化，提高蛇形机器人对环境的适应性，实现电机过载风险最小化，实现方式简单，通用性好。附图说明下面结合附图和实施例对本发明作进一步说明。图1是本发明的基于弹性包络的蛇形机器人安全攀爬控制方法的流程图；图2是本发明的基于弹性包络的蛇形机器人安全攀爬控制方法中采用的D-H分析法所建立的参考坐标系示意图；图3是本发明的基于弹性包络的蛇形机器人安全攀爬控制方法中所控制的正交连接蛇形机器人的三维模型图。具体实施方式参照图1，本发明提供了一种基于弹性包络的蛇形机器人安全攀爬控制方法，包括步骤：S1、依次控制蛇形机器人在不同控制参数下进行螺旋攀爬运动；本实施例中提到的控制参数指A和K，其中A表示幅度，K表示角度，A和K决定了蛇形机器人的螺旋攀爬运动的控制函数，该控制函数为：θi＝Ai·sin(ω·t+K·i)，i＝0,1…N-1θi表示第i个关节的转角，Ai表示第i个关节的幅度，Ai和K同决定蛇形机器人螺旋攀爬运动时的螺旋形状，Ai的取值范围为[0,π/2]，K取值范围为(π/5,π/2)；ω表示角速度，其决定蛇形机器人的攀爬速度。若A1＝A2＝…＝AN-1，则机器人将形成等距螺旋状，并且所环抱的螺旋半径相等。S2、采集蛇形机器人在每次螺旋攀爬过程中的位置和姿态，进而求解蛇形机器人的各关节的实时位姿；S3、根据求解获得的蛇形机器人的各关节位姿，计算出各关节与杆件柱心直线的间隙；S4、基于间隙二值图像获得不同时刻下蛇形机器人与杆件的弹性包络规律；S5、计算获得不同弹性包络规律下的攀爬系数，并计算不同控制参数下整个攀爬运动的平均攀爬系数；S6、获取所计算的多个平均攀爬系数的中间值所对应的控制参数，作为蛇形机器人在对应杆件上的安全攀爬控制参数；即获取攀爬系数的中间值所对应的A和K作为蛇形机器人的安全攀爬控制参数；S7、根据该安全攀爬控制参数控制蛇形机器人在对应杆件上进行攀爬运动。根据计算出来的控制参数，可以一步到位直接对蛇形机器人进行参数设置，根据该安全攀爬控制参数控制蛇形机器人在对应杆件上进行攀爬运动。进一步作为优选的实施方式，所述步骤S2，具体包括：S21、采用测距仪器采集蛇形机器人上的特定点到其所攀爬杆件的距离，同时采用陀螺仪采集蛇形机器人的蛇头关节模块的三维方向；S22、分别将测距仪器采集的距离和陀螺仪采集的三维方向作为蛇形机器人的位置和姿态，基于D-H分析法，求解获得蛇形机器人的各关节的实时位姿轨迹。进一步作为优选的实施方式，所述步骤S22，具体包括：S221、分别将测距仪器采集的距离和陀螺仪采集的三维方向作为蛇形机器人的位置和姿态，应用D-H坐标法建模，由空间约束方程得到N个独立空间约束方程，其中，N表示蛇形机器人的关节数量；S222、限定沿杆件轴线方向为蛇形机器人整体运动方向，消去一个自由度；S223、基于陀螺仪采集的三维方向补充3个约束方程；S224、基于测距仪器采集的距离补充1个约束方程；S225、通过以上N+4个约束方程，求解获得蛇形机器人的螺旋攀爬位姿，获得各关节的实时位姿轨迹。进一步作为优选的实施方式，所述步骤S3，其具体为：结合下式，根据求解获得的蛇形机器人的各关节位姿，计算出各关节与杆件柱心直线的间隙：且杆件柱心直线的方程为：上式中，Rli表示蛇形机器人的第i个关节到杆件柱心直线的间隙，(xi,yi,zi)和(xi+1,yi+1,zi+1)分别表示蛇形机器人的第i个关节的两关节点的坐标，(x0,y0,z0)表示杆件柱心的坐标，a、b和c均为常数。进一步作为优选的实施方式，所述步骤S4，具体包括：S41、将时间轴等分离散化，以离散时间点记录各关节与杆件柱心直线的间隙；S42、将间隙为0对应包络并标示为0，间隙大于0对应非包络并标示为1，进而将蛇形机器人的各关节的距离规律生成间隙二值图像；所述间隙二值图像中，像素横坐标从左到右对应表示从蛇头到蛇尾模块，像素纵坐标从下到上对应表示由小到大离散时刻，对每一时刻各关节的间隙为0/非0对应该时刻到下一时刻的像素点标为0/1；S43、根据生成的间隙二值图像获得不同时刻下蛇形机器人与杆件的弹性包络规律。应用间隙二值图像，可直观、有效地分析蛇形机器人螺旋攀爬运动过程中的弹性包络规律；对不同控制函数参数、不同杆径以及不同关节模块弹性体的弹性系数和厚度进行实验，应用间隙二值图像可进一步揭示蛇形机器人螺旋攀爬运动中不同控制函数参数和不同环境条件下(如杆径和关节模块与被攀爬杆件之间的摩擦系数)的弹性包络规律。进一步作为优选的实施方式，所述步骤S5，具体包括：S51、根据下式，计算蛇形机器人的各关节的弹性形变量：△ri＝Rli-ry-rg其中，△ri表示蛇形机器人第i个关节的弹性形变量，Rli表示蛇形机器人的第i个关节到杆件柱心直线的间隙，rg表示杆件的半径，ry表示关节块件的半径；S52、根据下式，分别计算蛇形机器人的抱紧力Fn和攀爬力f：上式中，k表示关节块件表面的弹性系数，u表示静摩擦系数，S53、根据下式，分别计算每个弹性包络规律下的攀爬系数λ：上式中，Gi表示第i个关节模块的重力。由此可见，本发明中，攀爬系数是根据抱紧力计算出来的值，抱紧力的大小决定了攀爬系数的大小。抱紧力太小，弹性包络效果差，各关节模块分担的克服重力的摩擦力分配均衡性差，部分关节模块之间需提供克服关节模块重力的支撑力大，可增加部分电机过载风险。增加抱紧力提升弹性包络效果，各关节模块分担的克服重力的摩擦力分配更均衡性，关节模块之间的支撑力更均衡，可降低电机过载风险。但是若抱紧力太大，可直接带来电机过载风险。具体的，本发明采用的D-H分析法的原理如下：D-H分析法是一种最常用的机器人运动学建模方法，由Denavit和Hartenberg在1955年共同提出。使用该方法进行运动学建模时可以忽略机器人的结构顺序和复杂程度，因此得到了广泛的应用。D-H表示法参考坐标系示意图如图2所示，使用D-H表示法建立机器人运动学模型的步骤如下：首先根据预先设定的方法为机器人的每个关节建立参考坐标系；然后按照一定的顺序对各个关节的参考坐标系进行旋转、平移变换后，将其变换到相邻关节的参考坐标系中；最后求得机器人全局坐标系到机器人任意关节的变换矩阵，将该关节到基座所经过的所有关节的变换矩阵相乘，所得的结果即为该关节到全局坐标系的变换矩阵。1)任意关节末端坐标表示机器人任意关节的末端坐标可以在该关节的参考坐标系空间中表示为：其中n＝[nx，ny，nz]T代表该关节的法线向量，o＝[ox，oy，oz]T代表该关节的指向向量，a＝[ax，ay，az]T代表该关节的接近向量，它们均为单位向量且满足n＝o×a；P＝[Px，Py，Pz]T则表示末端坐标系与基坐标系的相对位置。2)坐标系变换矩阵当参考坐标系绕X轴、Y轴、Z轴旋转或平移时，旋转或平移之后得到的矩阵为新坐标系的变换矩阵(分别为旋转矩阵或平移矩阵)。当机器人的关节绕该关节参考坐标系的X轴，Y轴和Z轴均旋转θ角度，其旋转矩阵为Rot(x,θ)、Rot(y,θ)和Rot(z,θ)，旋转后该关节的末端坐标表示矩阵可以由旋转矩阵Rot(x,θ)、Rot(y,θ)和Rot(z,θ)分别右乘旋转前的末端坐标表示矩阵得到。其中Rot(x,θ)、Rot(y,θ)、Rot(z,θ)为：当机器人的关节沿该关节参考坐标系的X轴，Y轴和Z轴分别平移x、y、z后，则平移后该关节的末端坐标表示矩阵可以由平移矩阵Trans(x,y,z)右乘平移前的末端坐标表示矩阵得到。其中为Trans(x,y,z)：3)D-H分析法D-H分析法是为了获得关节n坐标系到关节n+1坐标系的变换矩阵。将关节n的转动副轴线方向定义为Zn-1，并将其旋转角度定义为θn角；将Zn-1与关节n+1的转动副轴线方向Zn间的公垂线方向并指向Zn定义为Xn，用an表示该条公垂线的长度；Yn的方向由右手法则确定；用dn+1表示该公垂线与下一条公垂线在Zn轴方向上的距离；Zn-1与Zn之间的角度定义为用αn。用该方法在两连杆三关节机构上建立参考坐标系如图3-3所示。坐标系建立完后，按照以下步骤即可求得D-H分析法的转移矩阵：首先，将关节n绕Z轴旋转角度θn，使Xn-1与Xn相互平行；其次，沿X轴、Z轴方向分别平移an、dn；最后绕X轴旋转角度αn；即可求得关节n的坐标系到关节n+1的坐标系的变换矩阵：因此获得：图2中，如果求机器人的从起始关节到机器人第i个关节之间的变换矩阵，定义机器人的起始关节为第1个关节，已知机器人全局坐标系到机器人基座参考坐标系的变换矩阵为Tg的情况下，则第i个关节坐标系与机器人全局坐标系间的变换矩阵可以表示为：gTi＝Tg·T1·T2…Ti蛇形机器人运动学建模正交连接蛇形机器人三维模型如图3所示。基于D-H分析法对正交连接蛇形机器人进行运动学建模，首先需要为每个关节模块建立参考坐标系，定义关节模块i的参考坐标系为{Oi}，将关节模块i舵机转动轴的方向与关节模块i的中轴线的交点定义为参考坐标系{Oi}的原点Oi，而Xi轴就取关节模块i的中轴线，以指向下一个关节的方向为正方向。Zi轴取关节i的舵机的转动轴线，将副摆臂指向主摆臂定义为正方向。Yi轴由右手定则确定，即Xi×Yi＝Zi。按此规则在每一个关节上都建立与其固联的物体坐标系{Oi}。由于关节模块尾部没有固定基座，故基座参考坐标系{O0}可自由选取，为了研究方便，可以把基座参考坐标系{O0}定义为尾部关节的末端，姿态与尾部关节的参考坐标系{O1}相同，只是在X轴的方向上{O1}沿着{O0}的X0轴平移了l0的距离。建立好基座参考坐标系{O0}和各个关节的参考坐标系{Oi}后，结合正交关节蛇形机器人的结构特性，根据D-H分析法的定义，可以得出第i个关节参考坐标系通过以下四个步骤变换到第i+1个关节的参考坐标系：首先，参考坐标系{Oi}绕Zi轴转动θi角度；然后，参考坐标系{Oi}再沿着Zi轴平移di的距离；接着，参考坐标系{Oi}沿Xi轴平移ai的距离；最后，参考坐标系{Oi}绕Xi轴转动αi角度。仔细比较可以发现，其实θi角所对应的就是关节i的舵机输出的转动角度，而di实际上是等于零的，ai就等于关节的实际长度L。因为蛇形机器人是正交关节连接的，即相邻关节之间的舵机转动轴(对应于Z轴)是相互垂直的，所以αi角等于π/2。通过以上分析，可以得出正交连接蛇形机器人D-H分析法参数表，如下表1所示：表1蛇形机器人D-H分析法参数表相对关系θdaα关节1相对于基坐θ0d0L0α0关节2相对于关节1θ10Lπ/2关节3相对于关节2θ20Lπ/2关节4相对于关节3θ30Lπ/2……………关节N+1相对于关节Nθn0Lπ/2对于尾部关节，根据其D-H分析法参数，可求得相对于基座参考坐标系的变换矩阵：0T1＝Rot(z,θ0)·Trans(0,0,d0)·Trans(a0,0,0)·Rot(x,α0)除尾部关节以外，可以得出关节i+1相对于关节i的变换矩阵为：将上式展开得：因此，对于由N个关节模块组成的正交关节蛇形机器人，第i个关节模块相对于基座的总变换矩阵0Tn-2为：D-H分析法的其它细节可以参照现有技术中的相关文件，本申请不再赘述。以上是对本发明的较佳实施进行了具体说明，但本发明创造并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做出种种的等同变形或替换，这些等同的变型或替换均包含在本申请权利要求所限定的范围内。当前第1页1 2 3