一种变形机器人姿态变换最佳途径的确定方法

摘要

本发明公开了一种变形机器人姿态变换最佳途径的确定方法，它在变形机器人的姿态控制上引入了图论理论，将姿态变换问题转化为图论相关问题，这样，变形机器人就可按照姿态变形复杂度权值最小原则，当求出最佳姿态变换途径之后，控制系统就可以给变形机器人各关节的电机发送相应的驱动命令，使得变形机器人能够按照最佳变换途径依次变换其关节转角，实现其姿态的合理顺序变形，实现其不同姿态间的相互转换，针对不同环境情况，可以稳定、高效、合理地实现变形机器人的姿态变换，使其具有很好的环境适应性。

说明书

技术领域

本发明涉及变形机器人，尤其是一种变形移动机器人。

背景技术

针对不同复杂环境的变形机器人、尤其是变形移动机器人的研究越来越多，而且机器人变形的姿态也越来越多。例如，一种变形移动机器人，有四条带有驱动电机的履带(属于小腿)，每条履带都通过一加长杆(属于大腿)经由两个运动关节与机器人主体相连，其各个运动关节都采用电机驱动。通过各自围绕其关节的转动，每一条小腿和大腿都可实现不同角度的转动变化，从而组合成变形移动机器人的多种姿态，实现其不同的行走方式，以适应在不同环境状况时的运动需要。在变形移动机器人姿态变换中，有些姿态之间的变换相对容易；有些姿态之间的变换相对复杂，有些甚至需要经过另一些姿态作为中间变换姿态来实现。例如，由某姿态A到某姿态B，可能会途径A-C-B，也可能会途径A-D-F-B变换过程；为使得的转动幅度最小，同时保证重心的偏移最小，达到机器人姿态变换的能耗小，速度快，整个机身稳定的效果，需要确定从某姿态A到某姿态B最佳的变换途径。

但目前针对其姿态变形的科学合理性问题，仍没有一种科学有效的解决方法。如何设计一种变形移动机器人在姿态互变时的科学、合理的最佳变形途径，是我们必须要解决的问题。

发明内容

本发明针对变形移动机器人在姿态互变时的科学、合理的最佳变形途径，提出一种基于图论的变形机器人姿态变换最佳途径的确定方法。

一种变形机器人姿态变换最佳途径的确定方法，采取如下步骤：

第一步，确立变形机器人各种可能的变形姿态，并为其取一个对应图论顶点的名称代号(i＝1,2,3···,n)，同时，建立起表明各种姿态是否可以相互直接变换的顶点连通图；

第二步，确定各个顶点V_i(i＝1,2,3···,n)间姿态变换复杂度权值的计算方法：

对于可直接变换或直接连通的顶点，各顶点间姿态变换的复杂度权值由关节的运动角度及重心的偏移量两个因素来确定；

设变形机器人从某种姿态到另一种姿态变换时，相对于当前姿态，其4个大腿关节转动的角度分别定义为θ_b1，θ_b2，θ_b3，θ_b4，对应的4个小腿关节运动角度分别定义为θ_s1，θ_s2，θ_s3，θ_s4，机器人主体重心的水平偏移分量为X_d，重心的垂直偏移分量为Y_d，并且取X_dMax是水平方向上重心的最大偏移量，Y_dMax为垂直方向上重心的最大偏移量，则大腿关节平均运动角度为:

$>{\theta }_b=\frac{{\theta }_{b1}+{\theta }_{b2}+{\theta }_{b3}+{\theta }_{b4}}{4}---\left(1\right)$>

同理，小腿关节平均运动角度为:

$>{\theta }_s=\frac{{\theta }_{s1}+{\theta }_{s2}+{\theta }_{s3}+{\theta }_{s4}}{4}---\left(2\right)$>

将变量θ_b和θ_s进行归一化处理，即使得数据处于0到1之间；归一化处理的方法是：将所得的θ_b和θ_s数值除以360度；再将重心水平和垂直偏移分量分别除以各方向上的最大偏移量，同时，考虑垂直方向上重心上升或下降对电机的负荷不同，设定重心上升时Y_d为正，重心下落时Y_d为负，那么，重心水平偏移的复杂度权值通过式表示，重心垂直偏移的复杂度权值通过式表示，式中λ是介于0和之间的某个试验数值；再将机身上升时的复杂度权值稍微调大，将下降时的复杂度权值稍微调小，然后将重心分量合成为矢量p，则p矢量的模可以写成：

$>\left|p\right|=\sqrt{{\left(\frac{\left|\lambda +Y_d\right|}{Y_{dMax}}\right)}^2+{\left(\frac{X_d}{X_{dMax}}\right)}^2}---\left(3\right)$>

然后，根据变形时θ_b、θ_s和p对变形难易程度的影响和经验，确定它们各自在姿态变换复杂度权值中所占的比重，分别用常数w₁、w₂、w₃表示，其中w₁、w₂、w₃符合w₃>w₁>w₂的规律，最后为了使计算机可以准确快速地处理数据，可将计算出的姿态变换的复杂度权值放大100倍并取整，使其处于合理的整数范围内；

这样，各种姿态之间变换的复杂度权值计算公式即可写为:

$>W=(w_1\times \frac{{\theta }_b}{360}+w_2\times \frac{{\theta }_s}{360}+w_3\times |p\left|\right)\times 100---\left(4\right)$>

当机身参数一定时，就可由此获得各顶点间不同的姿态变换复杂度权值；上述的姿态变换复杂度权值代表了机器人从一种姿态变换到另一种姿态时的难易程度，权值越低，姿态之间的变换越容易，反之，则越难；

对于不可直接连通顶点间的权值以无穷大符号∞表示；

第三步，建立各顶点V_i(i＝1,2,3···,n)间的权值矩阵

把第二步计算方法得出各顶点间的权值，用W_ij分别表示由顶点V_i直接变换到顶点V_j时的权值，此时，会有对角线上i＝j的权值W_ij＝0；据此建立一个具有i×j(此处，i＝j)个元素的权值矩阵W:

$>W=\left(\begin{array}{cccc}{W}_{11}& W_{12}& ...& W_{1j}\\ W_{21}& W_{22}& ...& W_{2j}\\ .& .& & .\\ .& .& & .\\ .& .& & .\\ W_{i1}& W_{i2}& ...& W_{ij}\end{array}\right)---\left(5\right)$>

第四步，确定权值最小的变换途径

首先，设定V为可连通姿态变换图中所有顶点V_i(i＝1,2,3···,n)的集合；设定S_i为V一个动态子集，S_i集合用来在遍历循环中不断地加入这样一些顶点：以某V_i(i＝1,2,3···,n)为起始顶点时，采用遍历方法搜索其到达当前V-S_i集合中各顶点的权值之和为最小的途径，并且用变量Best[V_j](j＝1,2,3···,n)专门记录当前可搜索到的从起始顶点V_i变换到任意一顶点V_j的最佳姿态变换途径的权值之和；同时用变量ROAD[V_j]专门记录顶点V_j在其当前最佳姿态变换途径中对应的前一个顶点，记为ROAD[V_j]＝“前向顶点”，将各个Best[V_j](j＝1,2,3···,n)相互比较后，选出具有最小权值之和的那个顶点V_a，每次，将这样一个V_a放入S_i集合中；

然后，计算权值最小的变换途径，计算过程如下：

步骤1、初始化：确定起始顶点V_i(i＝1,2,3···,n)，和目标顶点V_end(end＝1,2,···,n)，将起始顶点V_i放入S_i集合中，此时，S_i＝{V_i}，并且将顶点V_i与V集合中每个顶点之间的权值各自放入Best[V_j](j＝1,2,3···,n)之中，作为起始顶点当前与各顶点之间最小的权值之和，此时有，Best[V_j]＝W_ij，同时将其各自对应的前向顶点设为起始顶点，即ROAD[V_j]＝V_i(j＝1,2,3···,n)；

步骤2、比较当前S_i集合中起始顶点V_i与当前V-S_i集合中所有顶点之间的变换途径，选出各个权值之和为最小的变换途径所对应的V-S_i集合中的顶点V_a，V_a所对应变换途径的权值之和应为Best[V_a]；

若V_a≠V_end，则将顶点V_a加入到当前S_i集合中，表示为S_i＝S_i∪{V_a}；

若V_a＝V_end，执行步骤4；

步骤3、计算当前S_i集合中起始顶点V_i途经顶点V_a时，到V-S_i集合中各个顶点V_j(j＝1,2,3···,n)的途径权值之和：Best[V_a]+W_aj，将其与原来不经过顶点V_a的各个权值之和Best[V_j]作对比，若对其中的某些顶点V_j存在：(Best[V_a]+W_aj)<Best[V_j]，则更新其对应的最小权值为Best[V_j]＝Best[V_a]+W_aj，相应更新V_j对应的前向顶点为ROAD[V_j]＝V_a，否则都不更新；

返回步骤2，继续执行；

步骤4、依次记录目标顶点V_end，及由ROAD[V_end]开始逆向回溯的各个前向顶点，直至ROAD[V_j]为起始顶点V_i为止，所记录途径的逆向过程，即为由起始顶点变换到目标顶点的最佳变换途径，算法结束。

本发明的有益效果是：

1、当求出最佳姿态变换途径之后，控制系统就可以给变形机器人各关节的电机发送相应的驱动命令，使得变形机器人能够按照最佳变换途径依次变换其关节转角，实现其姿态的合理顺序变形。

2、在变形机器人的姿态控制上引入了图论理论，将姿态变换问题转化为图论相关问题。这样，变形机器人就可按照姿态变形复杂度权值最小原则，实现其不同姿态间的相互转换，针对不同环境情况，可以稳定、高效、合理地实现变形机器人的姿态变换，使其具有很好的环境适应性。

附图说明

图1是本发明实施例的一个变形移动机器人基本构造图；

图2是为本发明实施例建立的顶点连通图；

图3是本发明实施例加入权值后的顶点连通图；

图4是本发明实施例寻找最佳路径的程序流程图；

图5-1至5-8是本发明实施例对应的八种变形姿态示意图。

图中：001-机身，002-大腿，003-小腿。

具体实施方式

本发明实施例为一变形移动机器人，机身001长度为648mm，机身001宽度为450mm，大腿002长度为360mm，小腿003长度为262mm，取X_dMax为机器人全部伸展开时整个机身001长度的四分之一：473mm，取Y_dMax为站立最高时整个机身001的重心高度：622mm，同时各个关节转角分别取90°或45°的整数倍计算。

为寻找实施例所述机器人姿态变形的最佳途径，采用本发明来确定其姿态最佳变形途径的过程如下程序：

第一步，确立变形移动机器人各种可能的8种变形姿态，并为其取一个对应图论顶点的名称代号，如表1所示，对应的八种变形姿态如图5-1至5-8所示；

表1：姿态与顶点名称对应表

同时，按照变形中是否会出现运动干涉等因素，由人工建立起表明各种姿态间是否可以相互直接变换的顶点连通图，顶点连通图如图2所示，图中箭头表明可以直接实现的不同顶点间的变换方向。

第二步，确定各个顶点间姿态变换复杂度权值的计算方法

大腿002关节平均运动角度为:

$>{\theta }_b=\frac{{\theta }_{b1}+{\theta }_{b2}+{\theta }_{b3}+{\theta }_{b4}}{4}---\left(1\right)$>

同理，小腿001关节平均运动角度为:

$>{\theta }_s=\frac{{\theta }_{s1}+{\theta }_{s2}+{\theta }_{s3}+{\theta }_{s4}}{4}---\left(2\right)$>

将变量θ_b和θ_s进行归一化处理，即使得数据处于0到1之间；归一化处理的方法是：将所得的θ_b和θ_s数值除以360度；再将重心水平和垂直偏移分量分别除以各方向上的最大偏移量，同时，考虑垂直方向上重心上升或下降对电机的负荷不同，设定重心上升时Y_d为正，重心下落时Y_d为负，那么，重心水平偏移的复杂度权值通过式表示，重心垂直偏移的复杂度权值通过式表示，式中λ是介于0和之间的某个试验数值；再将机身上升时的复杂度权值稍微调大，将下降时的复杂度权值稍微调小，然后将重心分量合成为矢量p，则p矢量的模可以写成：

$>\left|p\right|=\sqrt{{\left(\frac{\left|\lambda +Y_d\right|}{Y_{dMax}}\right)}^2+{\left(\frac{X_d}{X_{dMax}}\right)}^2}---\left(3\right)$>

然后，根据变形时θ_b、θ_s和p对变形难易程度的影响和经验，确定它们各自在姿态变换复杂度权值中所占的比重，分别用常数w₁＝0.8，w₂＝0.5，w₃＝1.0，λ＝10表示，机器人主体重心的水平偏移分量X_d及重心的垂直偏移分量Y_d可由相互变换的两种姿态实情计算获得，最后为了使计算机可以准确快速地处理数据，可将计算出的姿态变换的复杂度权值放大100倍并取整，使其处于合理的整数范围内；

这样，各种姿态之间变换的复杂度权值计算公式即可写为:

$>W=(w_1\times \frac{{\theta }_b}{360}+w_2\times \frac{{\theta }_s}{360}+w_3\times |p\left|\right)\times 100---\left(4\right)$>

根据所给的机身参数，可获得不同的姿态变换复杂度权值；上述的姿态变换复杂度权值代表了机器人从一种姿态变换到另一种姿态时的难易程度，权值越低，姿态之间的变换越容易，反之，则越难。

将上述计算的各姿态变换复杂度权值计入图2所示的顶点连通图中，加入权值后的顶点连通图如图3所示；

第三步，建立各顶点间的权值矩阵

将第二步计算得出的各顶点间权值，代入公式(5)，建立起如下权值矩阵W：

$>W=\left(\begin{array}{cccccccc}0& \infty & \infty & 21& \infty & 47& \infty & \infty \\ \infty & 0& \infty & \infty & \infty & \infty & \infty & 71\\ \infty & \infty & 0& 26& \infty & \infty & \infty & \infty \\ 21& \infty & 26& 0& 55& \infty & \infty & \infty \\ \infty & 89& \infty & 55& 0& \infty & 43& 42\\ 69& \infty & \infty & \infty & \infty & 0& \infty & \infty \\ \infty & \infty & \infty & \infty & 40& \infty & 0& \infty \\ 127& 56& \infty & 88& 42& \infty & \infty & 0\end{array}\right)$>

第四步，确定权值最小的变换途径

首先，设定V为可连通姿态变换图中所有顶点V_i(i＝1,2,3···,8)的集合；设定S_i为V一个动态子集，S_i集合用来在遍历循环中不断地加入这样一些顶点：以某V_i(i＝1,2,3···,8)为起始顶点时，采用遍历方法搜索其到达当前V-S_i集合中各顶点的权值之和为最小的途径，并且用变量Best[V_j](j＝1,2,3···,8)专门记录当前可搜索到的从起始顶点V_i变换到任意一顶点V_j的最佳姿态变换途径的权值之和；同时用变量ROAD[V_j]专门记录顶点V_j在其当前最佳姿态变换途径中对应的前一个顶点，记为ROAD[V_j]＝“前向顶点”，将各个Best[V_j](j＝1,2,3···,8)相互比较后，选出具有最小权值之和的那个顶点V_a，每次，将这样一个V_a放入S_i集合中；

以起始顶点V₄到目标顶点V₂的变换途径确定过程为例，说明确定权值最小变换途径的算法过程及其所对应的步骤，寻找最佳路径的程序流程图如图4所示：

步骤1：初始化：确定起始顶点为V₄，目标顶点为V₂，此时有V＝{V_i(i＝1,2,3,4,5,6,7,8)},S₄＝{V₄}，同时，Best[V₄]＝W₄₄＝0，Best[V₁]＝W₄₁＝21，Best[V₃]＝W₄₃＝26，Best[V₅]＝W₄₅＝55，对于不与V₄直接连通的其它顶点有Best[V_j]＝∞(j＝2,6,7,8)，同时将其各自对应的前向顶点设为起始顶点，即ROAD[V_j]＝V₄(j＝1,2,3,4,5,6,7,8)。

步骤2：比较当前S₄集合中起始顶点V₄与当前V-S₄集合中所有顶点之间的变换途径，可见，Best[V₁]＝21为最小，此时，V₁≠V₂，于是将顶点V₁添加到S₄集合中，当前，S₄＝{V₄,V₁}。

步骤3：计算当前S₄集合中起始顶点V₄途经顶点V₁时，到V-S₄集合中各个顶点V_j的权值之和：Best[V₁]+W_1j(j＝2,3,5,6,7,8)，将其与原来不经过顶点V₁的各个权值之和Best[V_j](j＝2,3,5,6,7,8)作对比，可发现，途经顶点V₁到V₆时有Best[V₁]+W₁₆＝21+47＝68，小于过去的Best[V₆]＝∞，故取Best[V₆]＝Best[V₁]+W₁₆＝68，同时记录其对应的前向顶点为ROAD[V₆]＝V₁。而途经顶点V₁到其它顶点的权值都是无穷大，故其它的Best[V_j](j＝2,3,5,7,8)及其对应的前向顶点不再更新。

步骤4：返回步骤2，比较当前S₄集合中起始顶点V₄与当前V-S₄集合中所有顶点之间的变换途径，可见，Best[V₃]＝26为最小，此时，V₃≠V₂，于是将顶点V₃添加到集合S₄中，当前，S₄＝{V₄,V₁,V₃}。

步骤5：返回步骤3，计算当前S₄集合中起始顶点V₄途经顶点V₃时，到V-S₄集合中各个顶点V_j的权值之和：Best[V₃]+W_3j(j＝2,5,6,7,8)，将其与原来不经过顶点V₃的各个权值之和Best[V_j](j＝2,5,6,7,8)作对比，可发现，途经顶点V₃到其它顶点的权值都是无穷大，故所有的Best[V_j](j＝2,5,6,7,8)及其对应的前向顶点都不更新。

步骤6：返回步骤2，比较S₄集合中起始顶点V₄与当前集合V-S₄中所有顶点的权值之和，可见，Best[V₅]＝55为最小，此时，V₅≠V₂，于是将顶点V₅添加到集合S₄中，当前，S₄＝{V₄,V₁,V₃,V₅}。

步骤7：返回步骤3：计算当前S₄集合中起始顶点V₄途经顶点V₅时，到V-S₄集合中各个顶点V_j的权值之和：Best[V₅]+W_5j(j＝2,6,7,8)，将其与原来不经过顶点V₅的各个权值之和Best[V_j](j＝2,6,7,8)作对比，可发现：

途经顶点V₅到V₂时有Best[V₅]+W₅₂＝55+89＝144，小于过去的Best[V₂]＝∞，故取Best[V₂]＝Best[V₅]+W₅₂＝144，同时记录其对应的前向顶点为ROAD[V₆]＝V₅；

途经顶点V₅到V₇时有Best[V₅]+W₅₇＝55+43＝98，小于过去的Best[V₇]＝∞，故取Best[V₇]＝Best[V₅]+W₅₇＝98，同时记录其对应的前向顶点为ROAD[V₇]＝V₅；

途经顶点V₅到V₈时有Best[V₅]+W₅₈＝55+42＝97，小于过去的Best[V₈]＝∞，故取Best[V₈]＝Best[V₅]+W₅₈＝97，同时记录其对应的前向顶点为ROAD[V₈]＝V₅。

而途经顶点V₅到顶点V₆的权值是无穷大，故Best[V₆]及其对应的前向顶点不再更新。

步骤8：返回步骤2，比较S₄集合中起始顶点V₄与当前集合V-S₄中所有顶点的复杂度权值之和，可见，Best[V₆]＝68为最小，此时，V₆≠V₂，于是将顶点V₆添加到集合S₄中，当前，S₄＝{V₄,V₁,V₃,V₅,V₆}。

步骤9：返回步骤3，计算当前S₄集合中起始顶点V₄途经顶点V₆时，到V-S₄集合中各个顶点V_j的权值之和：Best[V₆]+W_6j(j＝2,7,8)，将其与原来不经过顶点V₆的各个权值之和Best[V_j](j＝2,7,8)作对比，可发现，途经顶点V₆到其它顶点的权值之和都是无穷大，故所有的Best[V_j](j＝2,7,8)及其对应的前向顶点都不更新。

步骤10：返回步骤2，比较S₄集合中起始顶点V₄与当前集合V-S₄中所有顶点的权值之和，可见，Best[V₈]＝97最小，此时，V₈≠V₂，于是将顶点V₈添加到集合S₄中，当前，S₄＝{V₄,V₁,V₃,V₅,V₆,V₈}。

步骤11：返回步骤3，计算当前S₄集合中起始顶点V₄途经顶点V₈时，到V-S₄集合中各个顶点V_j的权值之和：Best[V₈]+W_8j(j＝2,7)，将其与原来不经过顶点V₈的各个权值之和Best[V_j](j＝2,7)作对比，可发现，途经顶点V₈到V₂时有Best[V₈]+W₈₂＝97+56＝153，大于过去的Best[V₂]＝144，故Best[V₂]及ROAD[V₂]都不更新。

而途经顶点V₈到顶点V₇的权值之和是无穷大，故Best[V₇]及ROAD[V₇]都不更新。

步骤12：返回步骤2，比较S₄集合中起始顶点V₄与当前集合V-S₄中所有顶点的权值之和，可见，Best[V₇]＝98最小，此时，V₇≠V₂，于是将顶点V₇添加到集合S₄中，当前，S₄＝{V₄,V₁,V₃,V₅,V₆,V₇,V₈}。

步骤13：返回步骤3，计算当前S₄集合中起始顶点V₄途经顶点V₇时，到V-S₄集合中各个顶点V_j的权值之和：Best[V₇]+W_7j(j＝2)，将其与原来不经过顶点V₇的各个权值之和Best[V_j](j＝2)作对比，可发现，途经顶点V₇到顶点V₂的权值之和是无穷大，故Best[V₂]及ROAD[V₂]都不更新。

步骤14：返回步骤2，比较S₄集合中起始顶点V₄与当前集合V-S₄中所有顶点的复杂度权值之和，可见，Best[V₂]＝144最小，此时，V₂＝V₂。执行步骤4。

步骤15：即发明内容的步骤4，依次记录目标顶点V₂，及由ROAD[V₂]开始逆向回溯的各个前向顶点，直至回溯到起始顶点V₄为止(有：ROAD[V₂]＝V₅→ROAD[V₅]＝V₄)。

则，所记录途径将依次为V₂→V₅→V₄，其逆向过程：V₄→V₅→V₂即为由V₄变换到V₂的最佳变换途径，算法结束。

本发明所涉及的图论知识及编程方法均为本领域的公知性知识，不再赘述。