基于遗传模拟退火算法的蛇形机器人运动控制方法与流程

本发明涉及蛇形机器人的控制领域，特别是涉及基于遗传模拟退火算法的蛇形机器人运动控制方法。

背景技术：

生物蛇在大自然中是一种独特的动物，它相对于其它的动物来说，最特别的地方在于它不需要腿的配合就可以完成各种所需的运动，如前进、后退、翻滚、蜿蜒、攀爬等。在各种复杂的环境中，如沙漠、泥泞、湖泊、草丛等，生物蛇由于其独特的机构及生理特点，使它可以适应各种不同的环境，在各种复杂的环境中运动自如。

蛇形机器人是一种模仿生物蛇、具有独特结构形式的仿生机器人。它与一般的足式机器人、轮式或履带式机器人相比，最大的特点就是它没有腿轮的结构，它的运动完完全全就是靠机体与环境的接触摩擦以及各个关节间运动的配合来实现的。其它结构形式的机器人具有运动速度快的特点，但是不能很好地适应各种复杂的环境，因此运用的范围有限。蛇形机器人具备了其它结构形式的机器人所不具备的优势。蛇形机器人可以在各种复杂的环境下进行运动，如管道、沼泽、夹缝等，因此蛇形机器人的运用范围更广。

此外，其它结构形式的机器人大多是靠轮子驱动，或者腿和足来移动，运动方式比较单一，运动形式相对简单。而蛇形机器人采用模块化的设计，可以实现各种二维和三维的运动，如蜿蜒、翻滚、行波、攀爬等，运动方式多样，运动形式复杂。为了实现蛇形机器人多种多样的运动，必须对其运动学与动力学进行理论上的深入研究，因此蛇形机器人具有理论研究价值。目前的技术中，虽然也有涉及到蛇形机器人的一些运动控制，但是其进行逆解的过程较为复杂，精度较低，而且可靠性较差，导致无法有效地对蛇形机器人进行运动控制。

技术实现要素：

为了解决上述的技术问题，本发明的目的是提供基于遗传模拟退火算法的蛇形机器人运动控制方法。

本发明解决其技术问题所采用的技术方案是：

基于遗传模拟退火算法的蛇形机器人运动控制方法，包括步骤：

获取蛇形机器人首关节的目标位置和目标姿态；

采用遗传模拟退火算法对蛇形机器人进行逆解，求解获得蛇形机器人的各关节转角；

蛇形机器人的驱动器获取求解获得的各关节转角并控制各关节进行转动。

进一步，所述采用遗传模拟退火算法对蛇形机器人进行逆解，求解获得蛇形机器人的各关节转角的步骤，包括：

S1、初始化遗传模拟退火算法的初始种群、初始温度以及退温操作函数；

S2、计算种群中每一个个体的被选中概率，并根据计算出的被选中概率对种群进行随机选择；

S3、对随机选择后的种群进行交叉操作后，再进行变异操作；

S4、判断是否满足预设的停止准则，若是，则结束遗传算法并输出最优个体作为蛇形机器人的转角矩阵，反之，对种群中的个体进行更新；

S5、更新迭代参数，将遗传模拟退火算法的温度t_k下降、遗传代数k增加以及种群数组更新后，返回步骤S2进行迭代。

进一步，所述步骤S1，包括：

S11、根据蛇形机器人的关节数，生成遗传模拟退火算法的初始种群，所述初始总群是由下式的初始个体构成的：

上式中，n表示蛇形机器人的关节数，R表示变异因子，且R中的每个元素均为随机生成的处于[0,1]范围内的随机数，Q表示解空间的向量，且Q中的第i个元素满足下式：

S12、响应于用户输入的数据初始化遗传模拟退火算法的初始温度，并设定如下式的退温操作函数：

t_k+1＝λ·t_k

上式中，t_k表示当前温度，t_k+1表示退温操作后的温度，λ表示衰减系数，其取值范围为0.5～0.99。

进一步，所述步骤S2，包括：

S21、设定遗传模拟退火算法的适应度函数；

S22、根据适应度函数，采用下式计算种群中每一个个体的被选中概率，并根据计算出的被选中概率对种群进行随机选择：

其中，f_min代表种群中个体适应度的最小值，f(i)代表第i个个体的适应度值，f_i(t_k)代表在当前温度t_k下该个体被选中的概率。

进一步，所述步骤S21，其具体为：

根据下式，设定遗传模拟退火算法的适应度函数f(Q)：

其中，ΔE(Q)表示预设的目标函数，其表达式如下：

上式中，P_j和θ_i分别代表蛇形机器人的齐次变化矩阵⁰T_n的位置元素和姿态元素，和分别表示蛇形机器人的目标位置和目标姿态，α和β均表示预设的权重因子。

进一步，所述步骤S3，包括：

S31、按照设定的交叉率，采用赌轮盘法对种群中的个体进行选择；

S32、对选择后的种群进行交叉操作；

S33、对交叉操作后的种群进行变异操作。

进一步，所述步骤S31，包括：

S311、按照设定的交叉率P_c在种群中进行搜索，计算出每一个个体的适应度值后，按从大到小的顺序进行排序，所述交叉率P_c为：

P_c＝0.7-w×N/t_k

其中，w代表比例系数，取值在0到1之间，N为种群规模；

S312、根据下式计算每一个个体的适应度值占总适应度值的比例P_k(Q_i)后，组成赌轮盘：

上式中，f(Q_i)表示第i个个体的适应度值；

S313、随机产生一个0～1之间的随机数，根据随机数在赌轮盘上所处的群尖选择对应的个体；

S314、重复执行步骤S313直到选取的个体达到种群容量的最大值。

进一步，所述步骤S32，包括：

S321、从选择后的种群中选择两个个体作为父代个体，进行染色体交换从而产生子代个体；

S322、根据下式计算出子代的变异因子R^*：

上式中，下标1、2表示个体的序号，R表示父代个体的变异因子，rand()是0～1间的随机数；

S323、根据下式计算新的解向量Q^*：

上式中，下标1、2表示个体的序号，Q表示父代个体的解向量。

进一步，所述步骤S33，包括：

S331、对交叉操作后的种群，在其个体中按照设定的变异概率P_m随机选择一个个体Q_j(θ_j1,θ_j2,…,θ_ji,…,θ_jn)，所述变异概率P_m为：

P_m＝t_k/K

其中，K为常数，满足在算法开始时让P_m等于0.7；

S332、通过下式对选择的个体进行变异：

其中，P_s为变异系数，其表达式为：

P_s＝D+(f_ave-f(Q_j))/f_ave

上式中，D＝(f_max-f_ave)/f_max，f_max为本代个体中适应度的最大值，f_ave为本代个体中适应度的平均值。

进一步，所述步骤S4中所述判断是否满足预设的停止准则的步骤，其具体为：

判断是否达到最大遗传代数，或者是否满足以下条件：

上式中，f(Q_i^*)表示第i个个体的适应度值，Q^*表示变异操作后的新的解向量。

本发明的有益效果是：本发明的基于遗传模拟退火算法的蛇形机器人运动控制方法，包括步骤：获取蛇形机器人首关节的目标位置和目标姿态；采用遗传模拟退火算法对蛇形机器人进行逆解，求解获得蛇形机器人的各关节转角；蛇形机器人的驱动器获取求解获得的各关节转角并控制各关节进行转动。本方法通过遗传模拟退火算法对蛇形机器人进行逆解，用遗传算法的搜索策略弥补模拟退火算法的精度问题，用模拟退火算法的全局搜索能力避免遗传算法陷入局部最优的困境。遗传模拟退火算法把两种算法的优势有机地结合起来，不仅能使算法的效率得到提高，还能增强算法的全局把控能力。基于此算法执行的蛇形机器人运动控制方法，精确度高，可靠性较高，可以有效地对蛇形机器人进行运动控制。

附图说明

下面结合附图和实施例对本发明作进一步说明。

图1是本发明的基于遗传模拟退火算法的蛇形机器人运动控制方法的流程图；

图2是本发明的基于遗传模拟退火算法的蛇形机器人运动控制方法的蛇形机器人的逆解步骤的流程图。

具体实施方式

参照图1，本发明提供了一种基于遗传模拟退火算法的蛇形机器人运动控制方法，包括步骤：

获取蛇形机器人首关节的目标位置和目标姿态；

采用遗传模拟退火算法对蛇形机器人进行逆解，求解获得蛇形机器人的各关节转角；

蛇形机器人的驱动器获取求解获得的各关节转角并控制各关节进行转动。求解获得蛇形机器人的各关节转角后，可以根据现有技术的控制水平，控制各关节进行转动。

进一步作为优选的实施方式，参照图2，所述采用遗传模拟退火算法对蛇形机器人进行逆解，求解获得蛇形机器人的各关节转角的步骤，包括：

S1、初始化遗传模拟退火算法的初始种群、初始温度以及退温操作函数；

S2、计算种群中每一个个体的被选中概率，并根据计算出的被选中概率对种群进行随机选择；

S3、对随机选择后的种群进行交叉操作后，再进行变异操作；

S4、判断是否满足预设的停止准则，若是，则结束遗传算法并输出最优个体作为蛇形机器人的转角矩阵，反之，对种群中的个体进行更新；

S5、更新迭代参数，将遗传模拟退火算法的温度t_k下降、遗传代数k增加以及种群数组更新后，返回步骤S2进行迭代。

进一步作为优选的实施方式，所述步骤S1，包括：

S11、根据蛇形机器人的关节数，生成遗传模拟退火算法的初始种群，所述初始总群是由下式的初始个体构成的：

上式中，n表示蛇形机器人的关节数，R表示变异因子，且R中的每个元素均为随机生成的处于[0,1]范围内的随机数，Q表示解空间的向量，且Q中的第i个元素满足下式：

S12、响应于用户输入的数据初始化遗传模拟退火算法的初始温度，并设定如下式的退温操作函数：

t_k+1＝λ·t_k

上式中，t_k表示当前温度，t_k+1表示退温操作后的温度，λ表示衰减系数，其取值范围为0.5～0.99。

进一步作为优选的实施方式，所述步骤S2，包括：

S21、设定遗传模拟退火算法的适应度函数；

S22、根据适应度函数，采用下式计算种群中每一个个体的被选中概率，并根据计算出的被选中概率对种群进行随机选择：

其中，f_min代表种群中个体适应度的最小值，f(i)代表第i个个体的适应度值，f_i(t_k)代表在当前温度t_k下该个体被选中的概率。

进一步作为优选的实施方式，所述步骤S21，其具体为：

根据下式，设定遗传模拟退火算法的适应度函数f(Q)：

其中，ΔE(Q)表示预设的目标函数，其表达式如下：

上式中，P_j和θ_i分别代表蛇形机器人的齐次变化矩阵⁰T_n的位置元素和姿态元素，和分别表示蛇形机器人的目标位置和目标姿态，α和β均表示预设的权重因子。蛇形机器人的齐次变化矩阵⁰T_n是根据旋量理论对蛇形机器人进行正运动学建模所获得的。

ΔE(Q)越小，适应度的值就越大，分母加上0.001是为了避免零分母的出现。

进一步作为优选的实施方式，所述步骤S3，包括：

S31、按照设定的交叉率，采用赌轮盘法对种群中的个体进行选择；

S32、对选择后的种群进行交叉操作；

S33、对交叉操作后的种群进行变异操作。

步骤S3实际执行的是遗传操作，遗传操作是整个算法的关键部分，遗传操作对上一代的个体进行选择，被选择的个体然后通过交叉和变异产生下一代的个体。在这个过程中充满了随机性与不确定性，但是在全局范围内又要有把控搜索方向能力。遗传操作包括选择、交叉和变异三个步骤，即对应步骤S31～S33。

进一步作为优选的实施方式，所述步骤S31，包括：

S311、按照设定的交叉率P_c在种群中进行搜索，计算出每一个个体的适应度值后，按从大到小的顺序进行排序，所述交叉率P_c为：

P_c＝0.7-w×N/t_k

其中，w代表比例系数，取值在0到1之间，N为种群规模；

交叉率P_c是交叉操作的过程中一个很重要的参数，影响着子代的多样性以及算法的搜索范围。如果交叉率P_c取得过高，则容易破坏优秀的个体，不利于算法的收敛。如果交叉率取得过低，则降低了个体的多样性使算法陷入局部最优，得出的解缺乏质量。在算法开始的时候温度值较高，交叉率接近于0.7，这时算法进行大范围的搜索。随着温度下降，交叉率也在下降，符合算法后期对优秀个体的保护原则但同时又具有一定的跳变能力。

S312、根据下式计算每一个个体的适应度值占总适应度值的比例P_k(Q_i)后，组成赌轮盘：

上式中，f(Q_i)表示第i个个体的适应度值；

S313、随机产生一个0～1之间的随机数，根据随机数在赌轮盘上所处的群尖选择对应的个体；

S314、重复执行步骤S313直到选取的个体达到种群容量的最大值。

进一步作为优选的实施方式，所述步骤S32，包括：

S321、从选择后的种群中选择两个个体作为父代个体，进行染色体交换从而产生子代个体；

S322、根据下式计算出子代的变异因子R^*：

上式中，下标1、2表示个体的序号，R表示父代个体的变异因子，rand()是0～1间的随机数；

S323、根据下式计算新的解向量Q^*：

上式中，下标1、2表示个体的序号，Q表示父代个体的解向量。

进一步作为优选的实施方式，所述步骤S33，包括：

S331、对交叉操作后的种群，在其个体中按照设定的变异概率P_m随机选择一个个体Q_j(θ_j1,θ_j2,…,θ_ji,…,θ_jn)，所述变异概率P_m为：

P_m＝t_k/K

其中，K为常数，满足在算法开始时让P_m等于0.7；

S332、通过下式对选择的个体进行变异：

其中，P_s为变异系数，其表达式为：

P_s＝D+(f_ave-f(Q_j))/f_ave

上式中，D＝(f_max-f_ave)/f_max，f_max为本代个体中适应度的最大值，f_ave为本代个体中适应度的平均值。

由此可以看出，每个个体的变异系数随着其适应度值而自适应变化。适应度大的个体变异系数就会变小，而适应度小的个体变异系数就会增大，这避免了最优解附近的个体受到破坏。

进一步作为优选的实施方式，所述步骤S4中所述判断是否满足预设的停止准则的步骤，其具体为：

判断是否达到最大遗传代数，或者是否满足以下条件：

上式中，f(Q_i^*)表示第i个个体的适应度值，Q^*表示变异操作后的新的解向量。因此，算法收敛的判断条件是达到最大遗传代数，或者，当算法迭代过程中出现适应度值满足大于500这个要求时算法终止，满足这个适应度值的个体就是可以近似看成所求优化问题的最优解。

本发明中，遗传模拟退火算法(GSA)把遗传算法和模拟退火算法进行并行整合，用遗传算法的搜索策略弥补模拟退火算法的精度问题，用模拟退火算法的全局搜索能力避免遗传算法陷入局部最优的困境。GSA把两种算法的优势有机地结合起来，不仅能使算法的效率得到提高，还能增强算法的全局把控能力。基于此算法执行的蛇形机器人运动控制方法，精确度高，可靠性较高，可以有效地对蛇形机器人进行运动控制。

以上是对本发明的较佳实施进行了具体说明，但本发明创造并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做出种种的等同变形或替换，这些等同的变型或替换均包含在本申请权利要求所限定的范围内。