【技术实现步骤摘要】
本专利技术属于机器人，尤其涉及一种足式机器人运动轨迹规划方法、系统、设备及终端。
技术介绍
1、足式机器人是仿生机器人，模仿自然界中有足类哺乳动物的运动，例如人类、猫科动物等，足式机器人拥有全向运动能力和复杂地形的通过能力。现如今，足式机器人的本体运动控制具有较高水准，具备了一定的实际动物运动能力，例如平移运动、转向运动、跳跃、崎岖地面行走，但是大部分的运动控制涉及的都是单一运动模式，真实世界中动物的灵活运动是各种运动模式的组合，当前的各种运动控制器距离实现动物的敏捷运动还有一段距离。
2、模型预测控制器被广泛应用于具有无碰撞约束条件的足式机器人运动控制中，来实现机器人的导航规划。但是模型预测控制器的预测周期极大地影响了控制效果，长预测周期导致求解花费的时间长，短预测周期易使优化求解陷入局部最优。基于学习的方法能够实现机器人的实时导航，但是需要预训练或使用数据集。这些数据集主要是在模拟环境中获取的，在实际环境中使用时会出现差异，而真实世界数据集的制作成本非常大。使用传统的基于采样或者搜索的路径规划方法生成的轨迹难以符合足式机器人复杂的动力学模型，因此在轨迹跟踪的过程中易出现较大误差或者跟踪速度慢。
3、通过上述分析，现有技术存在的问题及缺陷为：
4、(1)机器人实际运动缓慢：没有发挥出足式机器人运动速度潜力，为了保证轨迹跟踪效果而运动缓慢；
5、(2)机器人运动轨迹单一：大多是空间中的几何轨迹，只体现出避障能力，没有体现足式机器人运动的灵活性；
6、(3)规划算法实时性
技术实现思路
1、针对现有技术存在的问题，本专利技术提供了一种足式机器人运动轨迹规划方法、系统、设备及终端。
2、本专利技术是这样实现的，一种足式机器人运动轨迹规划方法，所述足式机器人的运动轨迹规划方法有三层，分别是：上层全局无碰撞轨迹生成模块，中层非线性动力学轨迹优化模块，底层模型预测控制器状态轨迹跟踪模块。上层模块在全局2d障碍地图上快速生成一条机器人质心运动的粗糙多项式轨迹，包含机器人运动的平面位置[x，y]、方向角θ以及轨迹持续时间t；中层模块根据搜索所得初始轨迹和机器人动力学，构建非线性优化问题，优化指标包括：避障代价、状态轨迹平滑度、机器人动力学限制(速度，加速度)、足式机器人全向运动约束和轨迹时间代价，优化变量为分段表达的多项式轨迹的系数以及每段轨迹的持续时间；底层模块将优化后的轨迹作为非线性模型预测控制器的期望质心时空状态轨迹，用该控制器跟踪机器人质心的运动轨迹，进行机器人本体的运动控制。
3、进一步，上层全局无碰撞轨迹生成中，规划的机器人状态为平面位置和方向角[x，y，θ]，将给定的均匀空间离散为g×g个网格，将每个网格与对应的状态p2d(idx，idy)＝[x，y，θ]关联，采样策略为：
4、x＝idx·grid+rand(-1，1)·bias
5、y＝idy·grid+rand(-1，1)·bias.
6、其中(idx，idy)是状态点的索引，grid是网格大小，g是离散网格数量，p2d是状态点，存储在状态点集roadmap中，状态点的总数量为g×g＝n个。
7、进一步，足式机器人可以全向运动，所以状态量[x，y，θ]分开考虑，将两个状态点的连接构建为一个最优边界值问题，初始状态给定为si＝[spi，svi]，是父节点的状态，终止状态为sf＝[spf，svf]，终止位置是子节点的位置，终止速度由求解得到，优化整个状态轨迹的能量j(t)＝∫tsa(t)2dt最小(即加速度积分最小)，使用庞德里亚金极大值原理，得到状态估计的显示解为：
8、
9、
10、
11、其中该问题的数值解需要轨迹时间t，通过给定参考线速度vref和角速度ωref设定参考时间t＝tref：＝max(||[δx，δy]||2/vref，δθ/ωref)。
12、进一步，非线性动力学轨迹优化中，优化指标包括：状态轨迹的平滑性代价，权重为λs、状态轨迹距离障碍物的代价，权重为λc和轨迹段的时间代价，权重为λt，将诸如最大速度、最大加速度的机器人动力学限制为机器人全向运动的限制为状态点处前后状态的连续性限制为作为优化问题的约束项。优化问题的求解变量为每段状态轨迹的多项式系数c和时间t：
13、
14、
15、
16、
17、
18、其中s(t)是状态变量x，y，θ的n阶多项式轨迹，n是多项式段数，j表示第j段，r是状态平滑性代价的正定权重矩阵，衡量三个状态变量之间的代价比重，t是轨迹段的时间向量，tj是第j段多项式的持续时间，cji表示第j段多项式的第n阶系数向量。
19、进一步，模型预测控制器状态轨迹跟踪中，构建非线性模型预测控制问题：
20、
21、
22、
23、g(x，u，t)＝0
24、h(x，u，t)＜0
25、其中x(t)和u(t)是状态变量和状态输入，φ(·)是终端状态约束代价函数，l(·)是轨迹跟踪的二次型代价函数，是当前观测状态。fc(·)、g(·)和h(·)分别是系统动态方程，等式约束和不等式约束。
26、在模型预测控制器中添加了速度安全约束，保证在快速运动时的稳定性，
27、
28、其中λ1和λθ衡量平移速度和角速度的权重，是安全阈值。
29、模型预测控制器根据期望状态轨迹和机器人的动力学模型，求解计算机器人关节电机的控制指令，实现机器人的自主运动。
30、本专利技术的另一目的在于提供一种应用所述足式机器人运动轨迹规划方法的足式机器人运动轨迹规划系统，包括：
31、上层模块，用于在全局障碍地图上快速生成一条机器人质心运动的粗糙多项式轨迹，包含机器人运动的平面位置[x，y]、方向角θ以及轨迹时间t；
32、中层模块，用于根据初始轨迹和机器人动力学，构建非线性优化问题，优化指标包括：避障代价、状态轨迹平滑度、机器人动力学限制、足式机器人全向运动约束和轨迹时间代价，优化变量为分段表达的轨迹多项式系数以及时间；
33、底层模块，用于将优化的轨迹作为非线性模型预测控制器的期望质心时空状态轨迹，进行机器人的具体运动控制。
34、本专利技术的另一目的在于提供一种计算机设备，计算机设备包括存储器和处理器，存储器存储有计算机程序，计算机程序被处理器执行时，使得处理器执行所述的足式机器人运动轨迹规划方法的步骤。
35、本专利技术的另一目的在于提供一种计算机可读存储介质，存储有计算机程序，计算机程序被处理器执行时，使得处理器执行所述的足式机器人运动轨迹规划方法的步骤。
36、本专利技术的另一目的在于提供一种信息数据处理终端，本文档来自技高网...
【技术保护点】
1.一种足式机器人运动轨迹规划方法，其特征在于，所述足式机器人的运动轨迹规划方法有三层，分别为：
2.如权利要求1所述的足式机器人运动轨迹规划方法，其特征在于，上层全局无碰撞轨迹生成中，规划的机器人状态为平面位置和方向角[x，y，θ]，将给定的均匀空间离散为g×g个网格，将每个网格与对应的状态P2D(idx，idy)＝[x，y，θ]关联，采样策略为：
3.如权利要求1所述的足式机器人运动轨迹规划方法，其特征在于，足式机器人可以全向运动，所以状态量[x，y，θ]分开考虑，将两个状态点的连接构建为一个最优边界值问题，初始状态给定为si＝[spi，svi]，是父节点的状态，终止状态为sf＝[spf，svf]，终止位置是子节点的位置，终止速度由求解得到，优化整个状态轨迹的能量J(T)＝∫0Tsa(t)2dt最小(即加速度积分最小)，使用庞德里亚金极大值原理，得到状态估计的显示解为：
4.如权利要求1所述的足式机器人运动轨迹规划方法，其特征在于，非线性动力学轨迹优化中，优化指标包括：状态轨迹的平滑性代价，权重为λs、状态轨迹距离障碍物的代价权重为λc和轨
5.如权利要求1所述的足式机器人运动轨迹规划方法，其特征在于，模型预测控制期望状态轨迹跟踪中，在模型预测控制器中添加了速度安全约束，
6.一种应用如权利要求1～5任意一项所述足式机器人运动轨迹规划方法的足式机器人运动轨迹规划系统，其特征在于，包括：
7.一种计算机设备，计算机设备包括存储器和处理器，存储器存储有计算机程序，计算机程序被处理器执行时，使得处理器执行如权利要求1～5任意一项所述的足式机器人运动轨迹规划方法的步骤。
8.一种计算机可读存储介质，存储有计算机程序，计算机程序被处理器执行时，使得处理器执行如权利要求1～5任意一项所述的足式机器人运动轨迹规划方法的步骤。
9.一种足式机器人的运动轨迹规划系统，其特征在于，该系统包括：
10.如权利要求9所述的足式机器人的运动轨迹规划系统，其特征在于，该系统包括：
...【技术特征摘要】
1.一种足式机器人运动轨迹规划方法，其特征在于，所述足式机器人的运动轨迹规划方法有三层，分别为：
2.如权利要求1所述的足式机器人运动轨迹规划方法，其特征在于，上层全局无碰撞轨迹生成中，规划的机器人状态为平面位置和方向角[x，y，θ]，将给定的均匀空间离散为g×g个网格，将每个网格与对应的状态p2d(idx，idy)＝[x，y，θ]关联，采样策略为：
3.如权利要求1所述的足式机器人运动轨迹规划方法，其特征在于，足式机器人可以全向运动，所以状态量[x，y，θ]分开考虑，将两个状态点的连接构建为一个最优边界值问题，初始状态给定为si＝[spi，svi]，是父节点的状态，终止状态为sf＝[spf，svf]，终止位置是子节点的位置，终止速度由求解得到，优化整个状态轨迹的能量j(t)＝∫0tsa(t)2dt最小(即加速度积分最小)，使用庞德里亚金极大值原理，得到状态估计的显示解为：
4.如权利要求1所述的足式机器人运动轨迹规划方法，其特征在于，非线性动力学轨迹优化中，优化指标包括：状态轨迹的平滑性代价，权重为λs、状态轨迹距离障碍物的代价权重为λc和...
【专利技术属性】
技术研发人员：朱力军，张文涛，徐兆辉，许绍航，蔡培原，
申请(专利权)人：华中科技大学，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。