1.本发明属于自动控制
技术领域：
：，特别涉及一种多关节蛇形机器人运动规划技术。
背景技术：
：：2.蛇形机器人是模仿自然界中蛇的生物特性的一种仿生机器人，通常由多个关节连接或者用柔性材料构成，具有很强的避障和环境适应能力。因此被广泛应用于一些狭窄复杂的工作环境中，例如狭窄管道中的搜索工作、坍塌环境中的救援工作以及口腔内部手术和人体器官的微创手术等。3.由于其超冗余自由度的存在，使得避障运动控制极为困难。针对蛇形机器人的运动规划问题，现应用广泛的方法是构造模仿生物蛇蜿蜒运动的曲线，通过一系列运动控制函数来对蛇形机器人进行运动控制。此类方法只能针对特定场景，可移植性差，且运动学逆解计算量大。4.q-learning是典型的model-free强化学习算法，通过q-table记录每个节点状态下采取动作的行为价值，以便于获得最大的总奖励。q-learning可采用时间差分迭代更新学习方法，使价值函数q逼近目标函数q*，可高效解决各类障碍物环境的路径搜索问题。5.顶尖随动算法记录蛇形机器人首端沿着指定路径的运动轨迹，其余关节依次跟随首端进行运动规划，可提高蛇形机器人的穿障能力。技术实现要素：6.为解决上述技术问题，本发明提出一种多关节蛇形机器人运动规划方法，采用强化学习路径规划算法与顶尖随动控制算法相结合，实现蛇形机器人轨迹规划与运动控制。7.本发明采用的技术方案为：一种多关节蛇形机器人运动规划方法，包括：8.s1、将蛇形机器人的环境状态空间离散化，以栅格法构建障碍物场景环境地图，得到环境模型；所述环境模型中包括若干栅格节点，蛇形机器人各关节对应的执行器的初始位置、目标点位置以及障碍物的位置信息均通过所在栅格节点进行位置确定；9.s2、利用强化学习算法搜索得到无碰撞的路径点集；10.s3、利用三次均匀b样条曲线将步骤s2得到的路径点集进行平滑处理；11.s4、通过杆长几何约束优化，采用等差寻点轨迹跟踪，求解蛇形机器人所有关节对应的执行器在下一运动步长期望的位置坐标；12.s5、利用蛇形机器人驱动空间到关节空间的正运动学，根据蛇形机器人所有关节对应的执行器在下一运动步长期望的位置坐标，解算出蛇形机器人所有关节对应的执行器的关节控制量；13.s6、基于关节空间闭环控制策略，控制蛇形机器人运动；14.s7、当蛇形机器人所有关节无碰撞地从各自初始位置运动到目标点位置，结束。15.所述基于关节空间闭环控制策略，控制蛇形机器人运动，具体为：将蛇形机器人头部关节对应的执行器的位置和关节控制量进行储存，后续关节对应的执行器利用顶尖随动方法控制关节进行运动。16.本发明的有益效果：本发明的方法根据栅格地图法，将障碍物环境状态空间离散化，采用基于贪婪策略进行动作选择的q-learning强化学习算法进行路径搜索，可以得到狭小空间、复杂环境下的无碰撞路径点集，再利用三次均匀b样条函数曲线对有限方向的路径进行平滑处理，通过杆长几何约束优化，在轨迹点集中求取求解蛇形机器人所有关节关键点下一运动步长期望的位置坐标，由工作空间中末端执行器规划跟踪点的位置信息，通过逆向运动学解算出各关节段关键点的期望位姿，进一步通过蛇形机器人驱动空间到关节空间的正运动学解算出各关节控制量，将蛇形机器人头部的位置和关节控制量进行储存，后续关节利用顶尖随动方法进行运动，采用关节空间的闭环控制策略，从而使得整个蛇形机器人能无碰撞、平稳地跟随期望轨迹到达工作位姿；本发明的方法具有以下优点：17.(1)能很好的适用于各类障碍物环境，在离散路径下的顶尖随动控制算法中，通过迭代步长和线性插值处理，即保证了运动精度和平顺性，也提高了算法运算效率；18.(2)该算法适用于多关节蛇形机器人的平面和空间运动，且算法效率也满足实时控制的需求。附图说明19.图1是本发明栅格地图环境模型；20.图2是强化学习搜索的路径图；21.图3强化学习训练收敛时寻径步数与学习次数的关系图；22.图4是b样条函数曲线平滑处理后的路径图；23.图5为蛇形机器人通过障碍物环境的运动规划过程状态；24.其中，(a)为初始状态，(b)、(c)、(d)、(e)为中间的避障运动状态，(f)为蛇形机器人头部到达目标点的状态；25.图6为蛇形机器人各关节在顶尖随动运动控制中的角位移轨迹曲线变化。具体实施方式26.为便于本领域技术人员理解本发明的技术内容，下面结合附图对本
发明内容进一步阐释。27.为提高蛇形机器人对不同复杂场景的运动规划效率，本发明采用强化学习路径规划算法与顶尖随动控制算法相结合，来解决多关节蛇形机器人轨迹规划与运动控制问题；这里每个关节表示一节蛇形机器人的身体，蛇形机器人的每节身体通过一个执行器控制其运动。具体包括以下步骤：28.s1：为了避免连续的高维环境模型导致强化学习难以收敛，本发明将蛇形机器人的环境状态空间离散化。以栅格法构建障碍物场景环境地图，栅格地图环境模型如图1所示，环境模型中包括若干栅格节点，蛇形机器人每个关节执行器的初始位置、最终要到达的目标点位置以及周围障碍物的位置信息均通过各自所在栅格节点的位置进行确定。单个栅格的尺寸为地图的精度。29.s2、通过强化学习搜索的路径图如图2所示，具体包括以下分步骤：30.s21：设计当前栅格节点路径搜索的动作空间为前、后、左、右四个方向。31.s22：初始化每个栅格节点状态-选择动作对的行为价值q(s,a)。即蛇形机器人每个关节执行器在栅格节点s对应的动作a取值可以为前、后、左、右四个方向中的某个动作。32.s23：采用ε贪婪策略进行动作选择，以1-ε的概率从当前栅格节点状态s选择最大行为价值的动作a。33.s24：执行动作a，与环境模型进行交互，通过环境模型获得报酬r，进入下一栅格节点状态s′，r通过报酬函数计算，报酬函数表达式为：[0034][0035]式中，r大于0代表奖励，r小于0代表处罚，rg为到达目标点获得的报酬值，本实施例中取值为100；rc为发生碰撞的报酬值，本实施例中取值为-100；ro为既没有撞到障碍物也没有达到终点时的报酬值，本实施例中取值为-1；dt(t)为t时刻当前节点与目标点之间的欧式距离；do(t)为t时刻当前节点与附近最近障碍物之间的欧式距离；dwin为判定到达目标点的欧式距离，本实施例中取值为1.5个栅格距离；dcol为判定与障碍物发生碰撞的欧式距离，本实施例中取值为1.5个栅格距离。[0036]s25：利用q-learning的学习规则更新q-table中的q函数值，学习规则如下：[0037]q(s,a)＝q(s,a) α(r(s,a) γmaxq(s′,a′)-q(s,a))[0038]式中，q(s,a)为当前状态节点的行为价值，r(s,a)为在状态s下采取动作a的报酬，q(s′,a′)为下一状态节点的行为价值，α为强化学习的学习率，代表蛇形机器人学习新知识的程度，α∈[0,1]，γ为强化学习中的折扣率，代表考虑未来报酬的程度，γ∈[0,1]。[0039]s26：判断下一状态节点s′是否是目标节点，若是，则执行步骤s27；否则重复s23、s24、s25，直到达到目标节点。[0040]s27：得到收敛最短的无碰撞路径点集，采用b样条函数插值平滑处理。强化学习训练收敛时寻径步数与学习次数的关系图如图3所示。[0041]s3：经三次均匀b样条曲线平滑处理后的轨迹点集为pn(j)，蛇形机器人末端执行器的当前时刻位置点为pt(e)。b样条函数曲线平滑处理后的路径图如图4所示。[0042]s4：通过条件minpt 1(e)-pt(e)-l，在轨迹点集中求取末端执行器下一时刻位置pt 1(e)，其中l为多关节蛇形机器人一节身体的长度。[0043]s5：同理，可根据蛇形机器人各关节执行器的当前时刻位置，求解所有蛇形机器人关节点执行器在下一时刻的位置。[0044]s6：利用蛇形机器人驱动空间到关节空间的运动学，通过s10和s11得到的期望位置坐标，可以解算出关节控制量，基于关节空间闭环控制策略，控制蛇形机器人运动，蛇形机器人通过障碍物环境的运动规划过程状态如图5(a)-(f)所示。[0045]将蛇形机器人头部的位置和关节控制量进行储存，后续关节利用顶尖随动方法进行运动，从而使整个蛇形机器人无碰撞从起始点运动到目标点，蛇形机器人各关节在顶尖随动运动控制中的角位移轨迹曲线变化如图6所示。[0046]s7：判断末端执行器下一时刻位置pt 1(e)是否为目标位置，若是则停止；否则重复s4、s5、s6，直到蛇形机器人达到目标位置。[0047]本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的权利要求范围之内。当前第1页12当前第1页12
技术领域：
：，特别涉及一种多关节蛇形机器人运动规划技术。
背景技术：
：：2.蛇形机器人是模仿自然界中蛇的生物特性的一种仿生机器人，通常由多个关节连接或者用柔性材料构成，具有很强的避障和环境适应能力。因此被广泛应用于一些狭窄复杂的工作环境中，例如狭窄管道中的搜索工作、坍塌环境中的救援工作以及口腔内部手术和人体器官的微创手术等。3.由于其超冗余自由度的存在，使得避障运动控制极为困难。针对蛇形机器人的运动规划问题，现应用广泛的方法是构造模仿生物蛇蜿蜒运动的曲线，通过一系列运动控制函数来对蛇形机器人进行运动控制。此类方法只能针对特定场景，可移植性差，且运动学逆解计算量大。4.q-learning是典型的model-free强化学习算法，通过q-table记录每个节点状态下采取动作的行为价值，以便于获得最大的总奖励。q-learning可采用时间差分迭代更新学习方法，使价值函数q逼近目标函数q*，可高效解决各类障碍物环境的路径搜索问题。5.顶尖随动算法记录蛇形机器人首端沿着指定路径的运动轨迹，其余关节依次跟随首端进行运动规划，可提高蛇形机器人的穿障能力。技术实现要素：6.为解决上述技术问题，本发明提出一种多关节蛇形机器人运动规划方法，采用强化学习路径规划算法与顶尖随动控制算法相结合，实现蛇形机器人轨迹规划与运动控制。7.本发明采用的技术方案为：一种多关节蛇形机器人运动规划方法，包括：8.s1、将蛇形机器人的环境状态空间离散化，以栅格法构建障碍物场景环境地图，得到环境模型；所述环境模型中包括若干栅格节点，蛇形机器人各关节对应的执行器的初始位置、目标点位置以及障碍物的位置信息均通过所在栅格节点进行位置确定；9.s2、利用强化学习算法搜索得到无碰撞的路径点集；10.s3、利用三次均匀b样条曲线将步骤s2得到的路径点集进行平滑处理；11.s4、通过杆长几何约束优化，采用等差寻点轨迹跟踪，求解蛇形机器人所有关节对应的执行器在下一运动步长期望的位置坐标；12.s5、利用蛇形机器人驱动空间到关节空间的正运动学，根据蛇形机器人所有关节对应的执行器在下一运动步长期望的位置坐标，解算出蛇形机器人所有关节对应的执行器的关节控制量；13.s6、基于关节空间闭环控制策略，控制蛇形机器人运动；14.s7、当蛇形机器人所有关节无碰撞地从各自初始位置运动到目标点位置，结束。15.所述基于关节空间闭环控制策略，控制蛇形机器人运动，具体为：将蛇形机器人头部关节对应的执行器的位置和关节控制量进行储存，后续关节对应的执行器利用顶尖随动方法控制关节进行运动。16.本发明的有益效果：本发明的方法根据栅格地图法，将障碍物环境状态空间离散化，采用基于贪婪策略进行动作选择的q-learning强化学习算法进行路径搜索，可以得到狭小空间、复杂环境下的无碰撞路径点集，再利用三次均匀b样条函数曲线对有限方向的路径进行平滑处理，通过杆长几何约束优化，在轨迹点集中求取求解蛇形机器人所有关节关键点下一运动步长期望的位置坐标，由工作空间中末端执行器规划跟踪点的位置信息，通过逆向运动学解算出各关节段关键点的期望位姿，进一步通过蛇形机器人驱动空间到关节空间的正运动学解算出各关节控制量，将蛇形机器人头部的位置和关节控制量进行储存，后续关节利用顶尖随动方法进行运动，采用关节空间的闭环控制策略，从而使得整个蛇形机器人能无碰撞、平稳地跟随期望轨迹到达工作位姿；本发明的方法具有以下优点：17.(1)能很好的适用于各类障碍物环境，在离散路径下的顶尖随动控制算法中，通过迭代步长和线性插值处理，即保证了运动精度和平顺性，也提高了算法运算效率；18.(2)该算法适用于多关节蛇形机器人的平面和空间运动，且算法效率也满足实时控制的需求。附图说明19.图1是本发明栅格地图环境模型；20.图2是强化学习搜索的路径图；21.图3强化学习训练收敛时寻径步数与学习次数的关系图；22.图4是b样条函数曲线平滑处理后的路径图；23.图5为蛇形机器人通过障碍物环境的运动规划过程状态；24.其中，(a)为初始状态，(b)、(c)、(d)、(e)为中间的避障运动状态，(f)为蛇形机器人头部到达目标点的状态；25.图6为蛇形机器人各关节在顶尖随动运动控制中的角位移轨迹曲线变化。具体实施方式26.为便于本领域技术人员理解本发明的技术内容，下面结合附图对本
发明内容进一步阐释。27.为提高蛇形机器人对不同复杂场景的运动规划效率，本发明采用强化学习路径规划算法与顶尖随动控制算法相结合，来解决多关节蛇形机器人轨迹规划与运动控制问题；这里每个关节表示一节蛇形机器人的身体，蛇形机器人的每节身体通过一个执行器控制其运动。具体包括以下步骤：28.s1：为了避免连续的高维环境模型导致强化学习难以收敛，本发明将蛇形机器人的环境状态空间离散化。以栅格法构建障碍物场景环境地图，栅格地图环境模型如图1所示，环境模型中包括若干栅格节点，蛇形机器人每个关节执行器的初始位置、最终要到达的目标点位置以及周围障碍物的位置信息均通过各自所在栅格节点的位置进行确定。单个栅格的尺寸为地图的精度。29.s2、通过强化学习搜索的路径图如图2所示，具体包括以下分步骤：30.s21：设计当前栅格节点路径搜索的动作空间为前、后、左、右四个方向。31.s22：初始化每个栅格节点状态-选择动作对的行为价值q(s,a)。即蛇形机器人每个关节执行器在栅格节点s对应的动作a取值可以为前、后、左、右四个方向中的某个动作。32.s23：采用ε贪婪策略进行动作选择，以1-ε的概率从当前栅格节点状态s选择最大行为价值的动作a。33.s24：执行动作a，与环境模型进行交互，通过环境模型获得报酬r，进入下一栅格节点状态s′，r通过报酬函数计算，报酬函数表达式为：[0034][0035]式中，r大于0代表奖励，r小于0代表处罚，rg为到达目标点获得的报酬值，本实施例中取值为100；rc为发生碰撞的报酬值，本实施例中取值为-100；ro为既没有撞到障碍物也没有达到终点时的报酬值，本实施例中取值为-1；dt(t)为t时刻当前节点与目标点之间的欧式距离；do(t)为t时刻当前节点与附近最近障碍物之间的欧式距离；dwin为判定到达目标点的欧式距离，本实施例中取值为1.5个栅格距离；dcol为判定与障碍物发生碰撞的欧式距离，本实施例中取值为1.5个栅格距离。[0036]s25：利用q-learning的学习规则更新q-table中的q函数值，学习规则如下：[0037]q(s,a)＝q(s,a) α(r(s,a) γmaxq(s′,a′)-q(s,a))[0038]式中，q(s,a)为当前状态节点的行为价值，r(s,a)为在状态s下采取动作a的报酬，q(s′,a′)为下一状态节点的行为价值，α为强化学习的学习率，代表蛇形机器人学习新知识的程度，α∈[0,1]，γ为强化学习中的折扣率，代表考虑未来报酬的程度，γ∈[0,1]。[0039]s26：判断下一状态节点s′是否是目标节点，若是，则执行步骤s27；否则重复s23、s24、s25，直到达到目标节点。[0040]s27：得到收敛最短的无碰撞路径点集，采用b样条函数插值平滑处理。强化学习训练收敛时寻径步数与学习次数的关系图如图3所示。[0041]s3：经三次均匀b样条曲线平滑处理后的轨迹点集为pn(j)，蛇形机器人末端执行器的当前时刻位置点为pt(e)。b样条函数曲线平滑处理后的路径图如图4所示。[0042]s4：通过条件minpt 1(e)-pt(e)-l，在轨迹点集中求取末端执行器下一时刻位置pt 1(e)，其中l为多关节蛇形机器人一节身体的长度。[0043]s5：同理，可根据蛇形机器人各关节执行器的当前时刻位置，求解所有蛇形机器人关节点执行器在下一时刻的位置。[0044]s6：利用蛇形机器人驱动空间到关节空间的运动学，通过s10和s11得到的期望位置坐标，可以解算出关节控制量，基于关节空间闭环控制策略，控制蛇形机器人运动，蛇形机器人通过障碍物环境的运动规划过程状态如图5(a)-(f)所示。[0045]将蛇形机器人头部的位置和关节控制量进行储存，后续关节利用顶尖随动方法进行运动，从而使整个蛇形机器人无碰撞从起始点运动到目标点，蛇形机器人各关节在顶尖随动运动控制中的角位移轨迹曲线变化如图6所示。[0046]s7：判断末端执行器下一时刻位置pt 1(e)是否为目标位置，若是则停止；否则重复s4、s5、s6，直到蛇形机器人达到目标位置。[0047]本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的权利要求范围之内。当前第1页12当前第1页12
再多了解一些
本文用于创业者技术爱好者查询，仅供学习研究，如用于商业用途，请联系技术所有人。
