CN116175553A - 履带式移动机械臂移动稳定性评估方法、装置和终端设备 - Google Patents

Abstract

本发明实施例涉及履带式移动机械臂技术领域，公开了一种机械臂移动稳定性评估方法、装置和终端设备。上述机械臂移动稳定性评估方法包括：基于履带式移动机械臂的结构，构建机械臂运动学模型，其中，履带式移动机械臂的结构包括移动平台、冗余机械臂、控制系统附件及车载IMU；基于机械臂运动学模型，通过牛顿‑欧拉法建立机械臂动力学模型；基于机械臂动力学模型，获取冗余机械臂对移动平台的力和力矩；根据冗余机械臂对移动平台的力和力矩，计算履带式移动机械臂的倾覆力矩，基于倾覆力矩评估履带式移动机械臂的移动稳定性。本发明提高了履带式移动机械臂在移动过程的移动稳定性评估效果。

Description

履带式移动机械臂移动稳定性评估方法、装置和终端设备

技术领域

本发明涉及履带式移动机械臂技术领域，具体涉及一种机械臂移动稳定性评估方法、装置和终端设备。

背景技术

履带式移动机械臂(以下简称移动机械臂)将机械臂和移动平台相结合，打破了传统机械臂工作空间固定的局限性，受到了学者们的广泛关注。但是移动机械臂不可避免地存在倾覆问题，同时冗余机械臂的某些运动可能会加剧这些问题，因此需要研究冗余机械臂的运动对移动平台产生的影响。

倾覆指的是移动机械臂绕其倾覆轴线产生向外旋转，致使系统发生翻倒，一旦发生倾翻，会造成机器人零部件的损坏、机器人失去控制、无法正常运行等一系列的问题。

现有研究大多通过提出一种标识作为倾覆稳定性的度量，而未探究移动机器人倾覆的根源。此外，倾覆稳定性的研究多集中于轮式移动机械臂，关于履带式移动机械臂倾覆稳定性的研究较少。

发明内容

有鉴于此，本发明实施例提供了一种履带式移动机械臂移动稳定性评估方法，提高了履带式移动机械臂在移动过程的移动稳定性评估效果。

为达到上述目的，本发明采用如下技术方案：

第一方面，本发明实施例提供了一种履带式移动机械臂移动稳定性评估方法，包括：基于履带式移动机械臂的结构，构建机械臂运动学模型，其中，履带式移动机械臂的结构包括移动平台、冗余机械臂、控制系统附件及车载IMU；基于机械臂运动学模型，通过牛顿-欧拉法建立机械臂动力学模型；基于机械臂动力学模型，获取冗余机械臂对移动平台的力和力矩；根据冗余机械臂对移动平台的力和力矩，计算履带式移动机械臂的倾覆力矩，基于倾覆力矩评估履带式移动机械臂的移动稳定性。

基于第一方面，在一些实施例中，基于履带式移动机械臂结构，构建机械臂运动学模型，包括：通过MDH法在冗余机械臂各关节处建立坐标系；根据坐标系及冗余机械臂各关节处的参数计算冗余机械臂的正向运动学的一般方程，构建机械臂运动学模型。

基于第一方面，在一些实施例中，冗余机械臂正向运动学的一般方程为：

其中，n_x、n_y、n_z，o_x、o_y、o_z，a_x、a_y、a_z，p_x、p_y、p_z分别为末端坐标系的X轴、Y轴、Z轴及原点在基坐标系下的矢量表示，

为相邻两关节坐标系i-1和i之间的变换矩阵。

基于第一方面，在一些实施例中，冗余机械臂包括多个机械臂连杆，基于机械臂运动学模型，通过牛顿-欧拉法建立机械臂动力学模型，包括：基于机械臂运动学模型计算多个机械臂连杆在相邻坐标系之间的坐标变换矩阵，根据多个机械臂连杆在相邻坐标系之间的坐标变换矩阵通过牛顿-欧拉公式前向递归计算多个机械臂连杆的速度和加速度；基于多个机械臂连杆的速度和加速度，通过牛顿-欧拉公式后向递归计算多个机械臂连杆的力和力矩，构建机械臂动力学模型。

基于第一方面，在一些实施例中，基于机械臂动力学模型，获取冗余机械臂对移动平台的力和力矩，包括：通过多个机械臂连杆的力和力矩计算出作用于机械臂第一个连杆的力和力矩；基于作用于机械臂第一个连杆的力和力矩计算出冗余机械臂对移动平台的力和力矩。

基于第一方面，在一些实施例中，根据冗余机械臂对移动平台的力和力矩，计算机械臂的倾覆力矩，基于倾覆力矩评估机械臂的移动稳定性，包括：获取倾覆运算数据；基于倾覆运算数据和冗余机械臂对移动平台的力和力矩，计算履带式移动机械臂各条倾覆轴线上的TOM倾覆力矩；通过TOM倾覆力矩判断履带式移动机械臂是否会发生倾覆，若TOM大于0时，履带式移动机械臂会发生倾覆；若TOM小于0时，履带式移动机械臂稳定，不会发生倾覆，且TOM小于0时其绝对值的大小与履带式移动机械臂的移动稳定性成正比。

基于第一方面，在一些实施例中，倾覆运算数据包括移动平台绕倾覆轴线方向旋转的角度数据、移动平台重心处的重力矢量及移动平台其余部件处的重力矢量，获取倾覆运算数据，包括：通过车载IMU获取移动平台绕倾覆轴线方向旋转的角度数据；基于冗余机械臂的安装位置和移动平台的重心位置，计算移动平台重心处的重力矢量和移动平台其余部件处的重力矢量。

第二方面，本发明实施例提供了一种履带式移动机械臂移动稳定性评估装置，包括：运动学模型建立模块，用于基于履带式移动机械臂的结构，构建机械臂运动学模型，其中，履带式移动机械臂的结构包括移动平台、冗余机械臂、控制系统附件及车载IMU；动力学模型建立模块，用于基于机械臂运动学模型，通过牛顿-欧拉法建立机械臂动力学模型；参数计算模块，用于基于机械臂动力学模型，获取冗余机械臂对移动平台的力和力矩；移动稳定性评估模块，用于根据冗余机械臂对移动平台的力和力矩，计算履带式移动机械臂的倾覆力矩，基于倾覆力矩评估履带式移动机械臂的移动稳定性。

第三方面，本发明实施例提供了一种终端设备，包括存储器、处理器以及存储在存储器中并可在处理器上运行的计算机程序，处理器执行计算机程序时实现如上述第一方面中任一项机械臂移动稳定性评估方法的步骤。

第四方面，本发明实施例提供了一种计算机可读存储介质，计算机可读存储介质存储有计算机程序，计算机程序被处理器执行时实现如上述第一方面中任一项机械臂移动稳定性评估方法的步骤。

本发明实施例中，通过建立机械臂运动学模型和动力学模型，解析出机械臂在运动过程中对移动平台产生的力和力矩，基于力和力矩的计算结果采用TOM倾覆力矩法评估了移动机械臂系统发生倾覆的可能性，提高了履带式移动机械臂在移动过程的移动稳定性评估效果。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例提供的履带式移动机械臂移动稳定性评估方法流程图；

图2是本发明实施例提供的冗余机械臂关节坐标系示意图；

图3是本发明实施例提供移动机械臂沿左、右倾覆轴线翻转时移动平台受力和力矩图；

图4是本发明实施例提供的移动机械臂沿前、后倾覆轴线翻转时移动平台受力和力矩图；

图5(a)至图5(e)是本发明实施例提供的移动机械臂攀爬楼梯仿真过程图；

图6是本发明实施例提供的移动机械臂车身倾斜角度随时间变化曲线；

图7(a)至图7(g)是本发明实施例提供的仿真过程中各关节的位置变化图；

图8是本发明实施例提供的仿真过程中倾覆力矩TOM值随时间变化示意图；

图9是本发明实施例提供的履带式移动机械臂移动稳定性评估装置示意图；

图10是本发明实施例提供的终端设备示意图。

具体实施方式

下面结合具体实施例对本发明进行更清楚的说明。以下实施例将有助于本领域的技术人员进一步理解本发明的作用，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。

为使本申请的目的、技术方案和优点更加清楚，下面将结合附图通过具体实施例来进行说明。

履带式移动机械臂(以下简称移动机械臂)将机械臂和移动平台相结合，打破了传统机械臂工作空间固定的局限性，受到了学者们的广泛关注。但是移动机械臂不可避免地存在倾覆性问题，同时冗余机械臂的某些运动可能会加剧这些问题，因此需要研究冗余机械臂的运动对移动平台产生的影响。

倾覆指的是移动机械臂绕其倾覆轴线产生向外旋转，致使系统发生翻倒，一旦发生倾翻，会造成机器人零部件的损坏、机器人失去控制、无法正常运行等一系列的问题。

对于上述问题，本发明提供了一种履带式移动机械臂移动稳定性评估方法，如图1所示，包括步骤101至步骤105。

步骤101：基于履带式移动机械臂的结构，构建机械臂运动学模型，其中，履带式移动机械臂的结构包括移动平台、冗余机械臂、控制系统附件及车载IMU。

在一些实施例中，本发明研究的履带式移动机械臂为自制的大负载、自重比、轻量化、高性能的柔性人机协作履带式移动机械臂，机械臂的结构包括移动平台、冗余机械臂、控制系统附件及车载IMU。通过MDH法在冗余机械臂各关节处建立坐标系，如图2所示。

根据坐标系及冗余机械臂各关节处的参数计算冗余机械臂的正向运动学的一般方程，构建机械臂运动学模型。

根据建立的坐标系及坐标系上各关节处的参数得到机械臂运动学模型参数如表1所示：

表1机械臂运动学模型参数表

在冗余机械臂正向运动学中，根据D-H参数可以得到相邻两坐标系之间的变换矩阵

为：

式中，Rot(z,θ_i)表示第i个关节坐标系绕Z轴的旋转矩阵；Trans(z,d_i)表示第i个关节坐标系沿Z轴的平移矩阵；Trans(x,l_i)表示第i个关节坐标系沿X轴的平移矩阵；

表示第i个关节坐标系绕X轴的旋转矩阵。

根据计算可得：

式中，c_i＝cosθ_i；s_i＝sinθ_i；i＝1、2……7。各个连杆的坐标系建立完成后，参数如下：a_i-1为沿着X_i-1轴，从Z_i-1轴移动到Z_i轴的距离；

为沿着X_i-1轴，从X_i-1轴旋转到Z_i轴的角度；d_i为沿着Z_i轴，从X_i-1轴移动到X_i轴的距离；θ_i为沿着Z_i轴，从X_i-1轴旋转到X_i轴的角度。

将所有变换矩阵依次相乘，可以得到冗余机械臂正向运动学的一般方程为：

式中，n_x、n_y、n_z，o_x、o_y、o_z，a_x、a_y、a_z，p_x、p_y、p_z分别为末端坐标系的X轴、Y轴、Z轴及原点在基坐标系下的矢量表示，

为相邻两关节坐标系i-1和i之间的变换矩阵。

步骤102：基于机械臂运动学模型，通过牛顿-欧拉法建立机械臂动力学模型。

由于移动机械臂的倾覆性判断对实时性要求较高，因此在考虑时间成本的前提下，本发明采用牛顿-欧拉法(Newton-Euler)建立机械臂动力学模型，通过递推得到整个机械臂系统的动力学方程。

其中牛顿方程如下：

F_i＝m_ia_Ci (4)

欧拉方程描述的是合力矩与角动量的导数之间的关系：

式中，F_i为机械臂连杆的质心力，m_i为连杆的重量，a_Ci为连杆的质心加速度，N_i为连杆i对质心的动量矩，I_Ci为刚体对其质心的惯性张量矩阵，ω_i为连杆i的角速度，

为连杆i的角加速度。

在一些实施例中，冗余机械臂包括多个机械臂连杆，基于机械臂运动学模型计算多个机械臂连杆在相邻坐标系之间的坐标变换矩阵，根据多个机械臂连杆在相邻坐标系之间的坐标变换矩阵通过牛顿-欧拉公式前向递归计算多个机械臂连杆的速度和加速度。

牛顿-欧拉公式中从连杆1到连杆n前向递归向外迭代计算连杆的速度和加速度如下：

角速度迭代：

角加速度迭代：

线速度迭代：

线加速度迭代：

式中：ω_i表示连杆i+1相对于连杆i的角速度；ⁱω_i、ⁱ⁺¹ω_i+1分别为连杆i和连杆i+1的角速度；

为连杆i上坐标系和连杆i+1上坐标系之间的坐标变换，基于机械臂运动学模型计算得出；/>

为连杆i+1的角速度；Z_i+1为连杆i+1的Z轴单位矢量；ⁱv_i和ⁱ⁺¹v_i+1分别为连杆i上坐标系原点和连杆i+1上坐标系原点的线速度；ⁱP_i+1为连杆i+1上坐标系原点在连杆i上坐标系中的位置。

基于多个机械臂连杆的速度和加速度，通过牛顿-欧拉公式后向递归计算多个机械臂连杆的力和力矩，构建机械臂动力学模型。

从连杆n到连杆1后向递归迭代计算连杆的相互作用力和力矩，如下：

/>

由于机械臂是一个复杂的耦合系统，故每个连杆除了受到质心的惯性力和惯性力矩之外，还受到相邻连杆的力和力矩。综合考虑，将连杆所受到的力和力矩相加，并将其重新排列可得到：

式中，ⁱf_i、ⁱ⁺¹f_i+1分别为作用于连杆i和连杆i+1的力，ⁱn_i、ⁱ⁺¹n_i+1分别为作用于连杆i和连杆i+1的力矩。

由于机械臂是一个多变量、强耦合的非线性系统，机械臂动力学模型中包含一些未知的参数，因此需要进行参数辨识，获取机械臂动力学模型中所有未知的惯性参数。选择CAD法进行参数辨识：直接从Solidworks中赋予材料、设定对应的坐标系，即可导出所有的惯性参数。利用全部的惯性参数进行速度和力的计算。

步骤103：基于机械臂动力学模型，获取冗余机械臂对移动平台的力和力矩。

在一些实施例中，通过多个机械臂连杆的力和力矩计算出作用于机械臂第一个连杆的力和力矩。基于作用于机械臂第一个连杆的力和力矩计算出冗余机械臂对移动平台的力和力矩。

具体的，通过式(6)、(7)、(8)、(9)内推得到每个连杆的角速度、角加速度、线速度、线加速度，然后通过式(10)、(11)外推求得第一个连杆的力和力矩，式(11)的结果即为作用于机械臂第一个连杆的力和力矩

M表示机械臂，机械臂对移动平台的力和力矩是六个维度，包括三个方向的力f_M和三个方向的力矩m_M。

机械臂的第一个连杆是和机械臂的移动平台相连的杆件，此力和力矩包含三个方向的分量，即f_M＝[f_X,f_Y,f_Z]^T，m_M＝[m_X,m_Y,m_Z]^T，求出作用于第一个连杆的力和力矩后，根据反作用力原理，相应的机械臂对移动平台的力和力矩为-W₁。

步骤104：根据冗余机械臂对移动平台的力和力矩，计算履带式移动机械臂的倾覆力矩，基于倾覆力矩评估履带式移动机械臂的移动稳定性。

基于倾覆力矩评估履带式移动机械臂的移动稳定性，需要对移动机械臂系统做出如下假设：(1)移动机械臂所在的表面不存在表面塌陷情况；(2)机械臂刚性安装在移动平台上，机械臂的连杆与移动平台均为刚性，不存在柔性变形；(3)忽略履带轮的变形情况。

在一些实施例中，倾覆运算数据包括移动平台绕倾覆轴线方向旋转的角度数据、移动平台重心处的重力矢量及移动平台其余部件处的重力矢量。

通过车载IMU获取移动平台绕倾覆轴线方向旋转的角度数据。基于冗余机械臂的安装位置和移动平台的重心位置，计算移动平台重心处的重力矢量和移动平台其余部件处的重力矢量。

如图3和图4所示为整个移动机械臂系统中移动平台上的力和力矩示意图，其中图3为移动机械臂沿左、右倾覆轴线翻转时移动平台受力和力矩图，图4为移动机械臂沿前、后倾覆轴线翻转时移动平台受力和力矩图。O_M为机械臂的安装位置，O_P为移动平台的重心位置，其受到的重力矢量为G_P；O_A为移动平台上其余部件的重心位置，其受到的重力矢量为G_A；α为移动平台绕着左、右倾覆轴线旋转的角度，β为移动平台绕着前、后倾覆轴线旋转的角度，α和β均为车载IMU读取到的绕世界坐标系旋转的角度数据转化为绕其倾覆轴线方向旋转的角度数据。

基于倾覆运算数据和冗余机械臂对移动平台的力和力矩，计算履带式移动机械臂各条倾覆轴线上的TOM倾覆力矩。

移动机械臂和地面接触的位置形成一个多边形，多边形的每两个相邻顶点的连线为一条倾覆轴线，移动机械臂的倾覆只会沿着倾覆轴线发生。分别对其左、右、前、后四条倾覆轴线进行分析，可以求出对应的倾覆力矩TOM₁、TOM₂、TOM₃和TOM₄。

其中，d为距离，d_P1表示O_P点到左倾覆轴线的距离；d_P2表示O_P点到后倾覆轴线的距离。f表示冗余机械臂作用在移动平台上的力，f_X、f_Y和f_Z分别表示X、Y、Z三个方向的力。M表示冗余机械臂作用在移动平台上的力矩，M_X、M_Y和M_Z分别表示X、Y、Z三个方向的力矩。

通过TOM倾覆力矩判断履带式移动机械臂是否会发生倾覆，同时计算四条倾覆力矩TOM₁、TOM₂、TOM₃和TOM₄，若其中任意一条倾覆力矩TOM大于0时，履带式移动机械臂会发生倾覆，且哪一条倾覆力矩大于0，就向哪一侧倾覆。若TOM均小于0时，履带式移动机械臂稳定，不会发生倾覆，且TOM小于0时其绝对值的大小与履带式移动机械臂的移动稳定性成正比。如果TOM等于0，则处于临界平衡状态，这种状态一般不存在，不列入考虑。

由于移动机械臂的倾覆只会沿着其中一条倾覆轴线发生，而且从车载IMU读取到的绕世界坐标系旋转的角度数据可以转化为绕其倾覆轴线方向旋转的α和β角度数据，因此可以根据α和β角的大小关系判断系统的倾覆方向，即沿着α和β角中较大角度的方向倾覆。

当移动平台压上障碍物时，相当于移动平台绕着倾覆轴线发生旋转运动，其α或者β角会增大，从式(12)中可以看出，TOM也会随之增大，即系统发生倾覆的可能性会增大。因此可以判断出移动机械臂系统是否会发生倾覆以及其倾覆的方向。

本发明通过建立机械臂运动学模型和动力学模型，解析出机械臂在运动过程中对移动平台产生的力和力矩，基于力和力矩的计算结果采用TOM倾覆力矩法评估了移动机械臂系统发生倾覆的可能性，提高了履带式移动机械臂在移动过程的移动稳定性评估效果。

实施例1

为验证本发明履带式移动机械臂移动稳定性评估方法的评估效果，进行了移动机械臂攀爬楼梯仿真实验，针对倾覆轴线进行分析，考虑机械臂运动的极限情况：机械臂在5s-8s内从竖直状态运动到水平向前状态，在10s-13s内从水平向前状态运动到水平向后状态，在15s-17s内从水平向后状态运动到竖直状态，研究在此过程中的移动机械臂状态的变化。采用的楼梯尺寸为：踏高为150mm，踏宽为290mm，坡度为27.45°。如图5所示为移动平台攀爬楼梯仿真过程。

图5中的(a)为攀爬楼梯的初始阶段，此时移动平台朝向楼梯方向运动，逐渐靠近楼梯底部，同时前摆臂摆动到接近楼梯坡度的角度；图5中的(b)所示为随着移动平台继续向前运动，前摆臂完全位于楼梯上；当前摆臂完全位于楼梯上时，后摆臂下压使得车身的倾斜角度和楼梯坡度接近，直到前摆臂和车身完全位于楼梯上，图5中的(c)所示；图5中的(d)所示为移动平台继续向前运动，前摆臂、车身和后摆臂均完全位于楼梯上；图5中的(e)所示为攀爬楼梯结束阶段，前摆臂首先爬过楼梯到达平面上，之后再将后摆臂下压到合适的角度以减小车身的倾斜角度，继续运动直到完全爬过楼梯。

图6所示为移动机械臂攀爬楼梯过程中车身倾斜角度随时间变化曲线。从图中可以看出，在移动平台完全位于楼梯上且继续运动时，车身的倾斜角度最大不超过31°，最小不超过25°，这说明移动过程中不会发生倾覆。

从Recurdyn仿真软件的后处理结果中导出各关节的位置、速度、加速度数据，其中各关节的位置如图7中的(a)～(g)所示。

将各关节的位置、速度、加速度数据代入式(12)中，可以求出整个仿真过程中的倾覆力矩TOM的值，如图8所示。

分析图8可知，在0-3s内：移动机械臂在平面进行运动，倾斜角度等于0，倾覆力矩TOM没有变化，恒等于初始状态的TOM_init；在3s-5s内：随着移动平台攀爬楼梯，其车身倾斜角度发生变化，导致当前状态下的TOM增大；在5s-8s内：随着机械臂从竖直状态摆到水平向前状态，此时的倾覆力矩TOM值不断减小；在10s-13s内：随着机械臂从水平向前状态运动到水平向后状态，此时的倾覆力矩TOM值不断增大；在15s-17s内：随着机械臂从水平向后状态运动到竖直状态，此时的倾覆力矩TOM值再次减小。与实验结果一直，在整个仿真过程中，倾覆力矩TOM值始终小于0，说明移动机械臂系统一直稳定，没有发生倾覆。

参见图9，本发明实施例提供了一种履带式移动机械臂移动稳定性评估装置90，包括：运动学模型建立模块910、动力学模型建立模块920、参数计算模块930和移动稳定性评估模块940。

运动学模型建立模块910，用于基于履带式移动机械臂的结构，构建机械臂运动学模型，其中，履带式移动机械臂的结构包括移动平台、冗余机械臂、控制系统附件及车载IMU。

动力学模型建立模块920，用于基于机械臂运动学模型，通过牛顿-欧拉法建立机械臂动力学模型。

参数计算模块930，用于基于机械臂动力学模型，获取冗余机械臂对移动平台的力和力矩。

移动稳定性评估模块940，用于根据冗余机械臂对移动平台的力和力矩，计算履带式移动机械臂的倾覆力矩，基于倾覆力矩评估履带式移动机械臂的移动稳定性。

图10是本发明一实施例提供的终端设备的示意图。如图10所示，该实施例的终端设备10包括：处理器100、存储器101以及存储在所述存储器101中并可在所述处理器100上运行的计算机程序102，例如履带式移动机械臂移动稳定性评估程序。所述处理器100执行所述计算机程序102时实现上述履带式移动机械臂移动稳定性评估方法实施例中的步骤，例如图1所示的步骤101至步骤104。或者，所述处理器100执行所述计算机程序102时实现上述各装置实施例中各模块/单元的功能，例如图9所示模块910至930的功能。

示例性的，所述计算机程序102可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器101中，并由所述处理器100执行，以完成本发明。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序102在所述终端设备10中的执行过程。例如，所述计算机程序102可以被分割成运动学模型建立模块、动力学模型建立模块、参数计算模块和移动稳定性评估模块。

所述终端设备10可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。所述终端设备可包括，但不仅限于，处理器100、存储器101。本领域技术人员可以理解，图10仅仅是终端设备10的示例，并不构成对终端设备10的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述终端设备还可以包括输入输出设备、网络接入设备、总线等。

所称处理器100可以是中央处理单元(CentralProcessingUnit，CPU)，还可以是其他通用处理器、数字信号处理器(DigitalSignalProcessor，DSP)、专用集成电路(ApplicationSpecificIntegratedCircuit，ASIC)、现成可编程门阵列(Field-ProgrammableGateArray，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

所述存储器101可以是所述终端设备10的内部存储单元，例如终端设备10的硬盘或内存。所述存储器101也可以是所述终端设备10的外部存储设备，例如所述终端设备10上配备的插接式硬盘，智能存储卡(SmartMediaCard，SMC)，安全数字(SecureDigital，SD)卡，闪存卡(FlashCard)等。进一步地，所述存储器101还可以既包括所述终端设备10的内部存储单元也包括外部存储设备。所述存储器101用于存储所述计算机程序以及所述终端设备所需的其他程序和数据。所述存储器101还可以用于暂时地存储已经输出或者将要输出的数据。

所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本发明的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。

本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

在本发明所提供的实施例中，应该理解到，所揭露的装置/终端设备和方法，可以通过其它的方式实现。例如，以上所描述的装置/终端设备实施例仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所述集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，RandomAccessMemory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。

以上所述实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围，均应包含在本发明的保护范围之内。

Claims (10)

1.一种履带式移动机械臂移动稳定性评估方法，其特征在于，包括：

基于履带式移动机械臂的结构，构建机械臂运动学模型，其中，所述履带式移动机械臂的结构包括移动平台、冗余机械臂、控制系统附件及车载IMU；

基于所述机械臂运动学模型，通过牛顿-欧拉法建立机械臂动力学模型；

基于所述机械臂动力学模型，获取所述冗余机械臂对所述移动平台的力和力矩；

根据所述冗余机械臂对所述移动平台的力和力矩，计算所述履带式移动机械臂的倾覆力矩，基于所述倾覆力矩评估所述履带式移动机械臂的移动稳定性。

2.如权利要求1所述的履带式移动机械臂移动稳定性评估方法，其特征在于，所述基于履带式移动机械臂结构，构建机械臂运动学模型，包括：

通过MDH法在所述冗余机械臂各关节处建立坐标系；

根据所述坐标系及所述冗余机械臂各关节处的参数计算所述冗余机械臂的正向运动学的一般方程，构建机械臂运动学模型。

3.如权利要求2所述的履带式移动机械臂移动稳定性评估方法，其特征在于，所述冗余机械臂正向运动学的一般方程为：

其中，n_x、n_y、n_z，o_x、o_y、o_z，a_x、a_y、a_z，p_x、p_y、p_z分别为末端坐标系的X轴、Y轴、Z轴及原点在基坐标系下的矢量表示，^i-1 _iT为相邻两关节坐标系i-1和i之间的变换矩阵。

4.如权利要求1所述的履带式移动机械臂移动稳定性评估方法，其特征在于，所述冗余机械臂包括多个机械臂连杆，所述基于所述机械臂运动学模型，通过牛顿-欧拉法建立机械臂动力学模型，包括：

基于所述机械臂运动学模型计算所述多个机械臂连杆在相邻坐标系之间的坐标变换矩阵，根据所述多个机械臂连杆在相邻坐标系之间的坐标变换矩阵通过牛顿-欧拉公式前向递归计算所述多个机械臂连杆的速度和加速度；

基于所述多个机械臂连杆的速度和加速度，通过所述牛顿-欧拉公式后向递归计算多个机械臂连杆的力和力矩，构建所述机械臂动力学模型。

5.如权利要求4所述的履带式移动机械臂移动稳定性评估方法，其特征在于，所述基于所述机械臂动力学模型，获取所述冗余机械臂对所述移动平台的力和力矩，包括：

通过所述多个机械臂连杆的力和力矩计算出作用于机械臂第一个连杆的力和力矩；

基于所述作用于机械臂第一个连杆的力和力矩计算出所述冗余机械臂对所述移动平台的力和力矩。

6.如权利要求1所述的履带式移动机械臂移动稳定性评估方法，其特征在于，所述根据所述冗余机械臂对所述移动平台的力和力矩，计算机械臂的倾覆力矩，基于所述倾覆力矩评估所述机械臂的移动稳定性，包括：

获取倾覆运算数据；

基于所述倾覆运算数据和所述冗余机械臂对所述移动平台的力和力矩，计算所述履带式移动机械臂各条倾覆轴线上的TOM倾覆力矩；

通过所述TOM倾覆力矩判断所述履带式移动机械臂是否会发生倾覆，若TOM大于0时，所述履带式移动机械臂会发生倾覆；若TOM小于0时，所述履带式移动机械臂稳定，不会发生倾覆，且TOM小于0时其绝对值的大小与所述履带式移动机械臂的移动稳定性成正比。

7.如权利要求6所述的履带式移动机械臂移动稳定性评估方法，其特征在于，所述倾覆运算数据包括所述移动平台绕所述倾覆轴线方向旋转的角度数据、所述移动平台重心处的重力矢量及所述移动平台其余部件处的重力矢量，所述获取倾覆运算数据，包括：

通过所述车载IMU获取所述移动平台绕所述倾覆轴线方向旋转的角度数据；

基于所述冗余机械臂的安装位置和所述移动平台的重心位置，计算所述移动平台重心处的重力矢量和所述移动平台其余部件处的重力矢量。

8.一种履带式移动机械臂移动稳定性评估装置，其特征在于，包括：

运动学模型建立模块，用于基于所述履带式移动机械臂的结构，构建机械臂运动学模型，其中，所述履带式移动机械臂的结构包括移动平台、冗余机械臂、控制系统附件及车载IMU；

动力学模型建立模块，用于基于所述机械臂运动学模型，通过牛顿-欧拉法建立机械臂动力学模型；

参数计算模块，用于基于所述机械臂动力学模型，获取所述冗余机械臂对所述移动平台的力和力矩；

移动稳定性评估模块，用于根据所述冗余机械臂对所述移动平台的力和力矩，计算所述履带式移动机械臂的倾覆力矩，基于所述倾覆力矩评估所述履带式移动机械臂的移动稳定性。

9.一种终端设备，包括存储器和处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至7任一项所述的履带式移动机械臂移动稳定性评估方法的步骤。

10.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至7中任一项所述履带式移动机械臂移动稳定性评估方法的步骤。

Images