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

摘要

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

说明书

技术领域

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

背景技术

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

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

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

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

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) 基础上，整机控制器对不同单体装置的运动学和动力学参数进行空间坐 标变换，并转换到全局坐标系下，使各个单体装置的运动学和动力学 参数可以转换到全局坐标，利用全局坐标统一实现仿蛇形软体机器人的 不同构型和运动。

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

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

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

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

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

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

式中，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为坐标转移矩阵。

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

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

其中，ⁱ⁺¹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)：

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

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

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

$\left(\begin{array}{c}{m}_i{\stackrel{..}{L}}_{\mathrm{ij}}+{2c}_i{\stackrel{.}{L}}_{\mathrm{ij}}+{2k}_i{\mathrm{\Delta L}}_{\mathrm{ij}}-c_i{\stackrel{.}{L}}_{(i-1)j}-k_i{\mathrm{\Delta L}}_{(i-1)j}+c_{i+1}({\stackrel{.}{L}}_{\mathrm{ij}}-{\stackrel{.}{L}}_{(i+1)j})+k_{i+1}({\mathrm{\Delta L}}_{\mathrm{ij}}-{\mathrm{\Delta L}}_{(i+1)j})\\ =F_{\mathrm{lij}}+f_{\mathrm{exti}}·e_{\mathrm{lij}}\end{array}\right)---\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为坐标转移矩阵。

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

${}^{i+1}v_0={}^{i}v_0+{\left({}^{i+1}R_i{}^{i+1}p_i\right)}^{\prime }{}^{i+1}a_0={}^{i}a_0+{}^{i+1}\stackrel{..}{R}_i{}^{i+1}p_i+2{}^{i+1}\stackrel{.}{R}_i{}^{i+1}v_i+{}^{i+1}R_i{}^{i+1}a_i---\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节坐标系下的表示。

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