买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：上海理工大学

摘要：本发明涉及一种仿生蛇形机器人主动避障的模型预测控制方法，满足真实机器人的物理约束以及在推导蛇形机器人简化的平均系统过程中的关节角变化速度慢的假设条件下，以最小化跟踪误差和保证全身与障碍物无碰撞为目的，建立目标函数；根据速度和方向误差以及蛇身的每个连杆和障碍物间的距离求得一组最优解用于控制。考虑蛇身全部连杆和障碍物的实际距离，避免全部关节与障碍物发生碰撞并且有效协调好稳定性与避障的行为冲突；实时调整蛇形机器人幅值大小，面对障碍物时通过缩减步态中的幅值大小减小所需转向角，降低不必要能量损耗；小角度避障，减小转向空间并且在完成避障任务后快速回归期望路径中；避免突加转向角后转弯步态不连续、性能不稳定等问题。

主权项：1.一种仿生蛇形机器人主动避障的模型预测控制方法，其特征在于，建立蛇形机器数学模型，在蛇形机器数学模型基础上建立模型预测控制器，满足真实机器人的物理约束以及在推导蛇形机器人简化的平均系统过程中的关节角变化速度慢的假设条件下，以最小化跟踪误差和保证全身与障碍物无碰撞为目的，建立目标函数；蛇形机器人在执行跟踪有障碍物的路径任务中，当经过固定采样时间后，模型预测控制器根据当前时刻的状态量，利用模型预测控制器中预测模型预测时间段内机器人的状态，并根据速度和方向误差以及蛇身的每个连杆和障碍物间的距离求得一组最优解，即在预测时间段内的每一时刻最优的输入量；模型预测控制器只更新当前时刻所需最优的输入量，并保持到下一采样时刻的到来，重复上述优化操作，完成路径跟踪和避障操作；所述蛇形机器数学模型建立方法：1蛇形机器人由n个长度为2l，质量为m，转动惯量为J＝ml23的连杆组成；第i连杆在惯性坐标系下的质心坐标为转动角度为即连杆与x轴的夹角，n个连杆角向量为第i关节转动角度为φi，它被定义为φi＝θi-θi+1，关节角向量为机器人在惯性坐标系下的质心坐标被定义为全部连杆坐标的均值，即机器人的前进方向用表示，被定义为连杆角的平均值，即2第一定律建立蛇形机器人的动力学模型如下： 式中fR是作用在连杆上的摩擦力，u是驱动力作为输入量，Mθ是惯性矩阵，Wθ和G是系数矩阵，Mθ＝JIn+ml2SθVSθ+ml2CθVCθ,Wθ＝ml2SθVCθ-ml2CθVSθ,G＝[lSθK-lCθK]，系数矩阵：V＝ATDDT-1A,K＝ATDDT-1D, In是n×n的单位矩阵，J为转动惯量；3假设作用在连杆i上的摩擦力[fR,xi,fR,yi]T为黏滞的，被描述为 式中ct和cn分别表示连杆的切向摩擦系数和法相摩擦系数，Rθi是连杆i的全局坐标到局部坐标的变换矩阵，作用在全部连杆的摩擦力表示为 式中 4利用平均角动量模型改写蛇形机器人数学模型：由蛇形机器人关节角和连杆角的定义可得以下关系：φ＝Dθ5代入式1中可得： 这表示输入量u对于关节角φ是全驱动的，因此，在对于蛇形机器人位置p和方向的控制时，不再使用输入量u，而是调节关节角φ；根据蛇形机器人的关节角和质心坐标的定义可得连杆位置z并求时间的微分，得到连杆的线性速度 为了从动力学模型1和2中推导出控制器设计所需的简化模型，引入一个新的变量对动力学模型进行改写： 改写后称之为平均角动量模型，对平均角动量模型8微分并代入式1中可以得到以下关系： 根据式4、式5、式7改写式2和式9得到 式7、式10和式11构成改写后蛇形机器人数学模型；所述在蛇形机器数学模型基础上建立模型预测控制器，即利用平均化法对蛇形机器数学模型推导出模型预测控制器设计所需的简易模型，简化方法如下：A：由于真实蛇形机器人的物理约束限制，模型预测控制器中添加有关节角变化限制，机器人关节角的幅值αt和偏移φ0t可被认为变化幅度很小，根据蛇形机器人的正弦蜿蜒步态，蛇形机器人的关节角i满足以下关系φi＝αsin{ωt+i-1δ}+φ013为了得到方向的模型，需要如下两个关系：1根据式5和式8可得关节角速度和连杆角速度的关系 式中2根据式5和方向角的定义可得 式中Γ＝DTDDT-1，同时对于和有如下定义： B：在使用平均化法简化模型前，先利用小角度近似法将部分模型线性化：假设关节角的角度很小，因此对于第i个关节，有和的近似关系，对于第i个关节和第j个关节，有的近似关系；根据小角度近似法，可得新的定义： 根据式14、方向角定义和小角度近似法得到的新的定义可得 同时，对式16微分，并代入式10可得 定义蛇形机器人速度v中的前向速度vt为沿着当前方向的质心速度的分量，侧向速度vn为垂直于沿着当前方向的质心速度的分量，即 将式15和式18代入式7、式11和式17可得 利用小角度近似法对式19-21简化，得到如下三个等式： C：利用平均化法从得到的式22、式23和式24推导出模型预测控制器设计所需的简易模型，对于如下系统 其中，函数f对t的周期为T，ε是一个很小的正参数，该系统的平均系统为 平均化法用系统25的平均系统的解逼近该系统的解，并且正参数ε越小，两系统的误差就越小；由于在设计模型预测控制器时，添加了关节角的变化约束并且考虑到真实机器人的物理约束，因此连杆速度机器人加速度和方向角角加速度可视为很小，系统22、23和24的解可以近似于以下平均系统的解 式中T＝2πω， 所述模型预测控制器建立及预测方法：首先从平均系统27-29中得到以下状态空间方程： 其中状态方程的状态量X＝[x1,...xn]T,Y＝[y1,...yn]T，输入量β＝[β1,β2]T＝[α,φo]T，状态方程各行表示为： 将状态空间方程30离散化，得到在t时刻用于模型预测控制器的预测模型：式中σ表示时间步长，Nh是预测时域，在t时刻对未来Nh个时刻的状态进行预测，具体的预测时间长度为sNh；初始状态为当前t时刻的状态ηt；根据预测模型得到的预测范围内的预测状态和输入量可以得到以下二次代价函数： 式中是距离的代价，是误差的代价，和是权值矩阵，其中k表示障碍物的数量,dij表示第i个连杆到第j个障碍物的距离，vref表示参考速度，参考角度；蛇形机器人的参考速度由期望指标给出，同时机器人通过当前位置和期望路径得到参考角度，因此，在已知障碍物位置的情况下，机器人在路径跟踪过程中主动避障问题将转为在t时刻求解以下优化问题： 其中为了满足真实机器人的物理约束以及在推导蛇形机器人简化的平均系统过程中的关节角变化速度慢的假设，在求解优化问题35时，添加如下约束条件： 其中j＝0,1,...,Nh-1，是一个给定的常数向量，表示优化问题35求出的关节角的幅值和偏移量的解绝对值必须满足在βmax范围内，同时其变化程度也要满足约束条件37。

全文数据：

权利要求：

百度查询： 上海理工大学 仿生蛇形机器人主动避障的模型预测控制方法