CN103978485A - 一种仿蛇型软体机器人控制系统和控制方法 - Google Patents

Abstract

本发明公开了一种仿蛇型软体机器人控制系统，该控制系统包括整机控制器以及至少一个单体装置，整机控制器通过无线通讯装置与单体装置通讯连接；单体装置包括顺次连接的无线通讯电路、单体控制器、运动控制电路以及电机，单体装置内部还安装有供电电池以及用于实时监控供电电池使用状态的电池管理电路。一种仿蛇形软体机器人控制方法，包括建立单体装置的运动学和动力学模型步骤和建立整体的仿蛇形软体机器人的运动学和动力学模型步骤。本发明的有益效果为：通过应用该仿蛇型软体机器人控制系统和控制方法可以灵活控制仿蛇型软体机器人运动，使得仿蛇型软体机器人可以广泛应用于医疗器械、救灾等多个技术领域。

Description

一种仿蛇型软体机器人控制系统和控制方法

技术领域

本发明涉及机器人技术领域，具体涉及一种仿蛇型软体机器人控制系统和控制方法。

背景技术

软体机器人技术是一项前瞻性的研究技术，在国内外都已经引起了相当广泛的关注，而传统的机器人主要是应用在工业的范围内，其本身的技术含量和成本比较高，不适用于日益增长的民用机器人领域的需求。

仿蛇型软体机器人主要是通过模仿蛇的结构和型状，采用软体材料或者多个模块制成，它们具有很高的自由度和几乎连续型变的能力，此类机器人构成的末端执行器，能够到达空间中的任意一点，并且对压力具有低阻抗，因此，它具有传统机器人无法比拟的环境适应性等特点，甚至可以通过主动变型和被动变型相结合，可以进入狭小的空间，适应多变的环境。例如，可以作为新型的医疗器械，进入口腔、肛门、胃等身体部位，以减少传统器械给病人带来的痛苦；可以进入地震、核辐射场所等危险系数高的环境和非常复杂的地方进行搜救工作；可以制造为人类服务的机器人，使得机器人在与人类沟通的时候减少危险性。

申请人曾经于欧盟软体机器人项目研究组工作多年，深入地了解了世界前沿课题的先进性，目前国内外各大科研机构都展开了相关的课题，然而，此类机器人的产品却寥寥无几，究其原因，广大的科学工作者只是集中在一种极高端的理论研究。随着电子产品的精密化、集成化和成本低廉化，其中很多的基础问题已经被解决，因此我们可以做到部分成果的产业化，希望在未来的软体机器人甚至服务机器人的产业化方向上起到一个先遣的作用，为新型的信息化产业在产业化方向做一个铺垫。

这个方向的市场前景非常广阔，理由有三：

1、信息和机器人技术的应用必将是下一次工业革命的核心力量；

2、真正推动机器人大面积应用和流行的先导力量必将是基础的机器人和电子信息的模块的集成；

3、新型柔性化，智能化的能够与人类共享工作空间，提供更好的服务质量的机器人产品已经是现阶段产品的迫切需求。

中国现在正处于机器人发展的青壮年期，机器人已经涉足了各个行业，从削面机器人，到扫地机器人，从医疗机器人到，救灾机器人，这些元素已经深入人心，然而我们采用基于仿蛇型软体机器人和其控制方法作为新的思考切入点，希望能够衍生出各类相关产品并且引导行业的动态发展，具有划时代的新颖性和强烈的现实需求。

发明内容

为解决现有机器人控制系统复杂的技术缺陷，本发明设计出一种仿蛇型软体机器人控制系统和控制方法，实现了简化机器人控制系统的目的，同时也避免了普通机器人运动不够灵活，不易控制的缺陷。

为实现上述目的，本发明采用的技术方案为：

一种仿蛇型软体机器人控制系统，该控制系统包括整机控制器以及至少一个单体装置，整机控制器通过无线通讯装置与单体装置通讯连接；单体装置包括顺次连接的无线通讯电路、单体控制器、运动控制电路以及电机，单体装置内部还安装有供电电池以及用于实时监控供电电池使用状态的电池管理电路。上述整机控制器，是整个仿蛇型软体机器人的核心控制部件，对整个机器人实现监控。单体装置和整体机器人的运动学和动力学模型以及机器人的运动控制都通过整机控制器来实现，通过整机控制器计算出每个单体装置的运动轨迹和距离并通过无线通讯装置传递给各个单体装置的运动控制电路，从而达到控制效果。同时整机控制器还监视各个单体装置的运行状态、外部环境、控制指令等，确保蛇型机器人在可控可靠状态下运行。供电电池是整个仿蛇型软体机器人的能量来源，根据机器人的尺寸和设计要求选择电池容量。电池管理电路是对电池进行监控的电路，如电池剩余电量监视、充电电流和电压控制、电池环境监视等。

优选的，无线通讯装置为Wi-Fi通讯装置。上述Wi-Fi通讯装置是仿蛇型软体机器人的神经，传递内部各单体装置运动的控制信号、运动状态数据、外部的控制指令等信号。

优选的，无线通讯电路为Wi-Fi电路。Wi-Fi电路用于接收整机控制器的控制信号。

优选的，运动控制电路为采用无传感器的矢量控制技术的伺服驱动器，该伺服驱动器包括依次连接的整流器、滤波器、制动器、IPM逆变器、信号采集单元、模拟运算器以及DSP控制器；整流器的输出端通过辅助电源与DSP控制器的输入端相连接；DSP控制器的输出端与IPM逆变器的控制端相连接。伺服驱动器还包括连接在DSP控制器的输入端的保护电路。伺服驱动器还包括连接在DSP控制器的输出端的显示器和远程通讯装置；其中，远程通讯装置包括分别接入智能仪表和CAN网络的联网通讯单元和CAN通讯单元。运动控制电路采用参数自获得技术自动计算永磁同步电机的参数，如定子电阻Rs、d轴电感量Ld和q轴电感量Lq，并能通过单体控制器计算电机的转速和位置，实现仿蛇型软体机器人的精确控制。

优选的，电机为正弦反电势的永磁同步电机。电机是仿蛇型软体机器人的动力来源，其高功率密度的优点能有效的减小机器人的体积和自身重量，使机器人更加灵活。

一种仿蛇形软体机器人控制方法，包括以下步骤：(1)建立单体装置的运动学和动力学模型步骤，整机控制器计算出每个单体装置的运动学和动力学参数并通过无线通讯装置传递给各个单体装置的运动控制电路；(2)建立整体的仿蛇形软体机器人的运动学和动力学模型，在步骤(1)基础上，整机控制器对不同单体装置的运动学和动力学参数进行空间坐标变换，并转换到全局坐标系下，使各个单体装置的运动学和动力学参数可以转换到全局坐标，利用全局坐标统一实现仿蛇形软体机器人的不同构型和运动。

进一步的，单体装置内部的运动学和动力学模型均采用并联模型；相邻单体装置间的运动学和动力学模型均采用串联模型。

进一步的，建立单体装置的运动学和动力学模型步骤中，采用的运动学算法如下式：

$T_{\mathrm{ia}}^{\mathrm{ib}}=\left[\begin{array}{cccc}1-{2\cos }^2{\mathrm{\θ}}_i{\sin }^2{\mathrm{\γ}}_i& -\sin {2\mathrm{\θ}}_i{\sin }^2{\mathrm{\γ}}_i& {\cos \mathrm{\θ}}_i\sin 2{\mathrm{\γ}}_i& {\cos \mathrm{\θ}}_i{\sin \mathrm{\γ}}_i{d}_i\\ -{\sin 2\mathrm{\θ}}_i{\sin }^2{\mathrm{\γ}}_i& 1-{\sin }^2{\mathrm{\θ}}_i{2\sin }^2{\mathrm{\γ}}_i& {\sin \mathrm{\θ}}_i{\sin 2\mathrm{\γ}}_i& {\sin \mathrm{\θ}}_i{\sin \mathrm{\γ}}_i{d}_i\\ -{\cos \mathrm{\θ}}_i{\sin 2\mathrm{\γ}}_i& -{\sin \mathrm{\θ}}_i{\sin 2\mathrm{\γ}}_i& {\cos 2\mathrm{\γ}}_i& {\cos \mathrm{\γ}}_i\mathrm{di}\\ 0& 0& 0& 1\end{array}\right]$

式中，θ_i，γ_i是旋转的角度，d_i是平移的距离。

进一步的，建立单体装置的运动学和动力学模型步骤中，在动力学建模方面，采用牛顿欧拉迭代法，建立多节串联软体机器人的模型，采用牛顿欧拉公式来描述第i个关节的力的关系，采用的动力学算法如下式：

$\begin{array}{c}{m}_i{\stackrel{..}{L}}_{\mathrm{ij}}+{2c}_i{\stackrel{.}{L}}_{\mathrm{ij}}+{2k}_i{\mathrm{\ΔL}}_{\mathrm{ij}}-c_i{\stackrel{.}{L}}_{(i-1)j}-k_i{\mathrm{\ΔL}}_{(i-1)j}+c_{i+1}({\stackrel{.}{L}}_{\mathrm{ij}}-{\stackrel{.}{L}}_{(i+1)j})+k_{i+1}({\mathrm{\ΔL}}_{\mathrm{ij}}-{\mathrm{\ΔL}}_{(i+1)j})\\ =F_{\mathrm{lij}}+f_{\mathrm{exti}}\·e_{\mathrm{lij}}\end{array}$

式中，m_i为关节i的质量，c_i和k_i分别为刚度系数和阻力系数，L_ij为伸长长度，f_exti是机构所受外力作用的合力，e_lij为长度方向上的单位矢量。

进一步的，建立整体的仿蛇形软体机器人的运动学和动力学模型步骤中，其中一个单体装置针对全局的运动学方程式如下：

[¹x_p ¹y_p ¹z_p 1]^T＝¹T_i[ⁱx_p ⁱy_p ⁱz_p 1]

式中，¹T_i＝¹T₁ ²T₂......ⁱT_i，¹x_p，¹y_p，¹z_p是第i个关节表现在全局坐标系下的坐标，ⁱx_p，ⁱy_p，ⁱz_p是第i个关节表现在第i个坐标系下的坐标，¹T_i为坐标转移矩阵。

进一步的，建立整体的仿蛇形软体机器人的运动学和动力学模型步骤中，其中一个单体装置针对全局的运动学方程式如下：

$v_0^{i+1}=v_0^i+{\left(R_i^{i+1}p_i^{i+1}\right)}^{\′}a_0^{i+1}=a_0^i+\stackrel{..}{R}_i^{i+1}p_i^{i+1}+2\stackrel{.}{R}_i^{i+1}v_i^{i+1}+R_i^{i+1}a_i^{i+1}$

其中，ⁱ⁺¹v₀为第i+1节的速度在全局坐标系的表示，ⁱv₀为第i节的速度在全局坐标系的表示，ⁱ⁺¹R_i第i节到第i+1节的旋转矩阵，ⁱ⁺¹p_i是从第i+1节的位置矢量在第i节坐标系的表示ⁱ⁺¹a₀是第i+1节的加速度，是第i节到第i+1节的旋转矩阵的二阶导数，是是第i节到第i+1节的旋转矩阵的一阶导数，ⁱ⁺¹v_i是第i+1节的速度在第i节坐标系的表示，ⁱ⁺¹a_i是第i+1节加速度在第i节坐标系下的表示。

本发明的有益效果为：通过应用该仿蛇型软体机器人控制系统和控制方法可以灵活控制仿蛇型软体机器人运动，使得仿蛇型软体机器人可以广泛应用于医疗器械、救灾等多个技术领域。

附图说明

图1为本发明所述仿蛇型软体机器人控制系统的连接示意图；

图2为本发明所述电池管理电路的连接示意图；

图3为本发明所述伺服驱动器的连接示意图。

具体实施方式

下面结合附图对本发明的结构进行详细解释说明。

如图1所示，本发明提供的一种仿蛇型软体机器人控制系统，该控制系统包括整机控制器以及至少一个单体装置，整机控制器通过无线通讯装置与单体装置通讯连接；单体装置包括顺次连接的无线通讯电路、单体控制器、运动控制电路以及电机，单体装置内部还安装有供电电池以及用于实时监控供电电池使用状态的电池管理电路。上述整机控制器，是整个仿蛇型软体机器人的核心控制部件，对整个机器人实现监控。单体装置和整体机器人的运动学和动力学模型以及机器人的运动控制都通过整机控制器来实现，通过整机控制器计算出每个单体装置的运动轨迹和距离并通过无线通讯装置传递给各个单体装置的运动控制电路，从而达到控制效果。同时整机控制器还监视各个单体装置的运行状态、外部环境、控制指令等，确保仿蛇型机器人在可控可靠状态下运行。供电电池是整个仿蛇型软体机器人的能量来源，根据机器人的尺寸和设计要求选择电池容量。电池管理电路是对电池进行监控的电路，如电池剩余电量监视、充电电流和电压控制、电池环境监视等。

仿蛇型软体机器人主要是通过模仿蛇的结构和型状，采用软体材料或者多个模块制成，仿蛇型软体机器人的控制核心是整机控制器，它是整个仿蛇型机器人的大脑，也是外界和机器人交互的接口。整机控制器加载了整个蛇型机器人的运动学和动力学模型和基于这个模型的控制算法。

如图2所示，本发明提供的电池管理电路，仿蛇形软体机器人由于使用环境的特苏醒，要求电源性能稳定、体积小、能量大、续航时间长，锂电池是符合这些要求的电源。但锂电池的充放电特点导致其管理要求较高，本仿蛇型软体机器人采用TI的BQ24032为基础设计电池管理电路。BQ24032通过外接电路和系统编程，达到对电池的充电和系统负载供电进行管理。由于锂电池充电特性和系统负载的多样性，电源管理电路具备以下功能：1、充电电流电压控制功能；2、充电终止和重新充电功能；3、在电源切换延时过程中保持系统电压稳定功能。

本发明提供的一种仿蛇型软体机器人控制系统，无线通讯装置为Wi-Fi通讯装置。上述Wi-Fi通讯装置是仿蛇型软体机器人的神经，传递内部各单体装置运动的控制信号、运动状态数据、外部的控制指令等信号。仿蛇型机器人的每个单体装置和整机控制器之间通过Wi-Fi通讯装置进行无线通讯，这样可以省去布线的复杂性和成本。仿蛇型机器人的人机交互界面采用VC++制作的界面，在界面上能够方便的输入仿蛇型机器人的运动路径和距离等参数，整机控制器能动态显示机器人的运动轨迹，并监视各个单体装置以及内部部件的工作状态，包括单体装置的运动角度、电机的输出电流、电路板的温度等。

本发明提供的一种仿蛇型软体机器人控制系统，无线通讯电路为Wi-Fi电路。Wi-Fi电路用于接收整机控制器的控制信号。

如图3所示，本发明提供的一种仿蛇型软体机器人控制系统，运动控制电路为采用无传感器的矢量控制技术的伺服驱动器，该伺服驱动器包括依次连接的整流器、滤波器、制动器、IPM逆变器、信号采集单元、模拟运算器以及DSP控制器；整流器的输出端通过辅助电源与DSP控制器的输入端相连接；DSP控制器的输出端与IPM逆变器的控制端相连接。伺服驱动器还包括连接在DSP控制器的输入端的保护电路。伺服驱动器还包括连接在DSP控制器的输出端的显示器和远程通讯装置；其中，远程通讯装置包括分别接入智能仪表和CAN网络的联网通讯单元和CAN通讯单元。运动控制电路采用参数自获得技术自动计算永磁同步电机的参数，如定子电阻Rs、d轴电感量Ld和q轴电感量Lq，并能通过单体控制器计算电机的转速和位置，实现仿蛇型软体机器人的精确控制。

本发明提供的一种仿蛇型软体机器人控制系统，电机为正弦反电势的永磁同步电机。电机是仿蛇型软体机器人的动力来源，其高功率密度的优点能有效的减小机器人的体积和自身重量，使机器人更加灵活。

本发明提供的一种仿蛇型软体机器人控制方法，包括以下步骤：(1)建立单体装置的运动学和动力学模型步骤，整机控制器计算出每个单体装置的运动学和动力学参数并通过无线通讯装置传递给各个单体装置的运动控制电路；(2)建立整体的仿蛇形软体机器人的运动学和动力学模型，在步骤(1)基础上，整机控制器对不同单体装置的运动学和动力学参数进行空间坐标变换，并转换到全局坐标系下，使各个单体装置的运动学和动力学参数可以转换到全局坐标，利用全局坐标统一实现仿蛇形软体机器人的不同构型和运动。

本发明提供的一种仿蛇型软体机器人控制方法，单体装置内部的运动学和动力学模型均采用并联模型；相邻单体装置间的运动学和动力学模型均采用串联模型。

本发明提供的一种仿蛇型软体机器人控制方法，建立单体装置的运动学和动力学模型步骤中，采用的运动学算法如式(1)：

$T_{\mathrm{ia}}^{\mathrm{ib}}=\left[\begin{array}{cccc}1-{2\cos }^2{\mathrm{\θ}}_i{\sin }^2{\mathrm{\γ}}_i& -\sin {2\mathrm{\θ}}_i{\sin }^2{\mathrm{\γ}}_i& {\cos \mathrm{\θ}}_i\sin 2{\mathrm{\γ}}_i& {\cos \mathrm{\θ}}_i{\sin \mathrm{\γ}}_i{d}_i\\ -{\sin 2\mathrm{\θ}}_i{\sin }^2{\mathrm{\γ}}_i& 1-{\sin }^2{\mathrm{\θ}}_i{2\sin }^2{\mathrm{\γ}}_i& {\sin \mathrm{\θ}}_i{\sin 2\mathrm{\γ}}_i& {\sin \mathrm{\θ}}_i{\sin \mathrm{\γ}}_i{d}_i\\ -{\cos \mathrm{\θ}}_i{\sin 2\mathrm{\γ}}_i& -{\sin \mathrm{\θ}}_i{\sin 2\mathrm{\γ}}_i& {\cos 2\mathrm{\γ}}_i& {\cos \mathrm{\γ}}_i\mathrm{di}\\ 0& 0& 0& 1\end{array}\right]---\left(1\right)$

式中，θ_i，γ_i是旋转的角度，d_i是平移的距离。

本发明提供的一种仿蛇型软体机器人控制方法，建立单体装置的运动学和动力学模型步骤中，在动力学建模方面，采用牛顿欧拉迭代法，建立多节串联软体机器人的模型，采用牛顿欧拉公式来描述第i个关节的力的关系，采用的动力学算法如式(2)：

$\begin{array}{c}{m}_i{\stackrel{..}{L}}_{\mathrm{ij}}+{2c}_i{\stackrel{.}{L}}_{\mathrm{ij}}+{2k}_i{\mathrm{\ΔL}}_{\mathrm{ij}}-c_i{\stackrel{.}{L}}_{(i-1)j}-k_i{\mathrm{\ΔL}}_{(i-1)j}+c_{i+1}({\stackrel{.}{L}}_{\mathrm{ij}}-{\stackrel{.}{L}}_{(i+1)j})+k_{i+1}({\mathrm{\ΔL}}_{\mathrm{ij}}-{\mathrm{\ΔL}}_{(i+1)j})\\ =F_{\mathrm{lij}}+f_{\mathrm{exti}}\·e_{\mathrm{lij}}\end{array}---\left(2\right)$

式中，m_i为关节i的质量，c_i和k_i分别为刚度系数和阻力系数，L_ij为伸长长度，f_exti是机构所受外力作用的合力，e_lij为长度方向上的单位矢量。

本发明提供的一种仿蛇型软体机器人控制方法，建立整体的仿蛇形软体机器人的运动学和动力学模型步骤中，其中一个单体装置针对全局的运动学方程式如式(3)：

[¹x_p ¹y_p ¹z_p 1]^T＝¹T_i[ⁱx_p ⁱy_p ⁱz_p 1]   (3)

式中，¹T_i＝¹T₁ ²T₂......ⁱT_i，¹x_p，¹y_p，¹z_p是第i个关节表现在全局坐标系下的坐标，ⁱx_p，ⁱy_p，ⁱz_p是第i个关节表现在第i个坐标系下的坐标，¹T_i为坐标转移矩阵。

本发明提供的一种仿蛇型软体机器人控制方法，建立整体的仿蛇形软体机器人的运动学和动力学模型步骤中，其中一个单体装置针对全局的运动学方程式如下：

$v_0^{i+1}=v_0^i+{\left(R_i^{i+1}p_i^{i+1}\right)}^{\′}a_0^{i+1}=a_0^i+\stackrel{..}{R}_i^{i+1}p_i^{i+1}+2\stackrel{.}{R}_i^{i+1}v_i^{i+1}+R_i^{i+1}a_i^{i+1}---\left(4\right)$

其中，ⁱ⁺¹v₀为第i+1节的速度在全局坐标系的表示，ⁱv₀为第i节的速度在全局坐标系的表示，ⁱ⁺¹R_i第i节到第i+1节的旋转矩阵，ⁱ⁺¹p_i是从第i+1节的位置矢量在第i节坐标系的表示ⁱ⁺¹a₀是第i+1节的加速度，是第i节到第i+1节的旋转矩阵的二阶导数，是是第i节到第i+1节的旋转矩阵的一阶导数，ⁱ⁺¹v_i是第i+1节的速度在第i节坐标系的表示，ⁱ⁺¹a_i是第i+1节加速度在第i节坐标系下的表示。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明实质内容上所作的任何修改、等同替换和简单改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种仿蛇型软体机器人控制系统，其特征在于，该控制系统包括整机控制器以及至少一个单体装置，整机控制器通过无线通讯装置与单体装置通讯连接；单体装置包括顺次连接的无线通讯电路、单体控制器、运动控制电路以及电机，单体装置内部还安装有供电电池以及用于实时监控供电电池使用状态的电池管理电路。

2.根据权利要求1所述的仿蛇型软体机器人控制系统，其特征在于，无线通讯装置为Wi-Fi通讯装置；无线通讯电路为Wi-Fi电路。

3.根据权利要求1所述的仿蛇型软体机器人控制系统，其特征在于，运动控制电路为采用无传感器的矢量控制技术的伺服驱动器，该伺服驱动器包括依次连接的整流器、滤波器、制动器、IPM逆变器、信号采集单元、模拟运算器以及DSP控制器；整流器的输出端通过辅助电源与DSP控制器的输入端相连接；DSP控制器的输出端与IPM逆变器的控制端相连接；伺服驱动器还包括连接在DSP控制器的输入端的保护电路；伺服驱动器还包括连接在DSP控制器的输出端的显示器和远程通讯装置；其中，远程通讯装置包括分别接入智能仪表和CAN网络的联网通讯单元和CAN通讯单元。

4.根据权利要求1所述的仿蛇型软体机器人控制系统，其特征在于，电机为正弦反电势的永磁同步电机。

5.一种仿蛇形软体机器人控制方法，其特征在于，包括以下步骤：

(1)建立单体装置的运动学和动力学模型步骤，整机控制器计算出每个单体装置的运动学和动力学参数并通过无线通讯装置传递给各个单体装置的运动控制电路；

(2)建立整体的仿蛇形软体机器人的运动学和动力学模型，在步骤(1)基础上，整机控制器对不同单体装置的运动学和动力学参数进行空间坐标变换，并转换到全局坐标系下，使各个单体装置的运动学和动力学参数可以转换到全局坐标，利用全局坐标统一实现仿蛇形软体机器人的不同构型和运动。

6.根据权利要求5所述的仿蛇形软体机器人控制方法，其特征在于，单体装置内部的运动学和动力学模型均采用并联模型；相邻单体装置间的运动学和动力学模型均采用串联模型。

7.根据权利要求6所述的仿蛇形软体机器人控制方法，其特征在于，建立单体装置的运动学和动力学模型步骤中，采用的运动学算法如下式：

$T_{\mathrm{ia}}^{\mathrm{ib}}=\left[\begin{array}{cccc}1-{2\cos }^2{\mathrm{\θ}}_i{\sin }^2{\mathrm{\γ}}_i& -\sin {2\mathrm{\θ}}_i{\sin }^2{\mathrm{\γ}}_i& {\cos \mathrm{\θ}}_i\sin 2{\mathrm{\γ}}_i& {\cos \mathrm{\θ}}_i{\sin \mathrm{\γ}}_i{d}_i\\ -{\sin 2\mathrm{\θ}}_i{\sin }^2{\mathrm{\γ}}_i& 1-{\sin }^2{\mathrm{\θ}}_i{2\sin }^2{\mathrm{\γ}}_i& {\sin \mathrm{\θ}}_i{\sin 2\mathrm{\γ}}_i& {\sin \mathrm{\θ}}_i{\sin \mathrm{\γ}}_i{d}_i\\ -{\cos \mathrm{\θ}}_i{\sin 2\mathrm{\γ}}_i& -{\sin \mathrm{\θ}}_i{\sin 2\mathrm{\γ}}_i& {\cos 2\mathrm{\γ}}_i& {\cos \mathrm{\γ}}_i\mathrm{di}\\ 0& 0& 0& 1\end{array}\right]$

式中，θ_i，γ_i是旋转的角度，d_i是平移的距离。

8.根据权利要求7所述的仿蛇形软体机器人控制方法，其特征在于，建立单体装置的运动学和动力学模型步骤中，在动力学建模方面，采用牛顿欧拉迭代法，建立多节串联软体机器人的模型，采用牛顿欧拉公式来描述第i个关节的力的关系，采用的动力学算法如下式：

$\begin{array}{c}{m}_i{\stackrel{..}{L}}_{\mathrm{ij}}+{2c}_i{\stackrel{.}{L}}_{\mathrm{ij}}+{2k}_i{\mathrm{\ΔL}}_{\mathrm{ij}}-c_i{\stackrel{.}{L}}_{(i-1)j}-k_i{\mathrm{\ΔL}}_{(i-1)j}+c_{i+1}({\stackrel{.}{L}}_{\mathrm{ij}}-{\stackrel{.}{L}}_{(i+1)j})+k_{i+1}({\mathrm{\ΔL}}_{\mathrm{ij}}-{\mathrm{\ΔL}}_{(i+1)j})\\ =F_{\mathrm{lij}}+f_{\mathrm{exti}}\·e_{\mathrm{lij}}\end{array}$

式中，m_i为关节i的质量，c_i和k_i分别为刚度系数和阻力系数，L_ij为伸长长度，f_exti是机构所受外力作用的合力，e_lij为长度方向上的单位矢量。

9.根据权利要求8所述的仿蛇形软体机器人控制方法，其特征在于，建立整体的仿蛇形软体机器人的运动学和动力学模型步骤中，其中一个单体装置针对全局的运动学方程式如下：

[¹x_p ¹y_p ¹z_p 1]^T＝¹T_i[ⁱx_p ⁱy_p ⁱz_p 1]

式中，¹T_i＝¹T₁ ²T₂......ⁱT_i，¹x_p，¹y_p，¹z_p是第i个关节表现在全局坐标系下的坐标，ⁱx_p，ⁱy_p，ⁱz_p是第i个关节表现在第i个坐标系下的坐标，¹T_i为坐标转移矩阵。

10.根据权利要求9所述的仿蛇形软体机器人控制方法，其特征在于，建立整体的仿蛇形软体机器人的运动学和动力学模型步骤中，其中一个单体装置针对全局的动力学方程式如下：

$v_0^{i+1}=v_0^i+{\left(R_i^{i+1}p_i^{i+1}\right)}^{\′}a_0^{i+1}=a_0^i+\stackrel{..}{R}_i^{i+1}p_i^{i+1}+2\stackrel{.}{R}_i^{i+1}v_i^{i+1}+R_i^{i+1}a_i^{i+1}$

其中，ⁱ⁺¹v₀为第i+1节的速度在全局坐标系的表示，ⁱv₀为第i节的速度在全局坐标系的表示，ⁱ⁺¹R_i第i节到第i+1节的旋转矩阵，ⁱ⁺¹p_i是从第i+1节的位置矢量在第i节坐标系的表示ⁱ⁺¹a₀是第i+1节的加速度，是第i节到第i+1节的旋转矩阵的二阶导数，是是第i节到第i+1节的旋转矩阵的一阶导数，ⁱ⁺¹v_i是第i+1节的速度在第i节坐标系的表示，ⁱ⁺¹a_i是第i+1节加速度在第i节坐标系下的表示。