一种连续型机器人姿态控制系统的制作方法

本发明属于自动控制技术领域，特别是涉及一种连续型机器人姿态控制系统。

背景技术：

传统的离散型机器人均采用刚性关节和连杆结构，用于实现在自由空间内的多自由度运动，但由于其自由度数目有限，因此对工作空间受限的环境适应性不强。与离散型机器人不同，连续型机器人为“无脊椎”的柔性结构，该机器人采用形状可以灵活改变的结构，而不具有任何刚性的关节和连杆。这种新型的仿生机器人具有良好的弯曲性能，可以柔顺而灵活地改变自身的形状，因此具有能根据环境障碍物的状况而改变自身形状的能力，对工作空间受限的环境具有独特的适应能力。另外，其应用前景广阔，可以应用于飞机油箱检查、多障碍物工业环境内的作业、弯曲管道和塌陷建筑物内的侦查和搜救、核电站内部管路的维护、人体疾病的诊疗等场合。

对连续型机器人的空间运动进行控制时，首先需要对机器人到达目标区域的行进路径进行规划，然后实现对给定的路径进行跟随。连续型机器人采用线驱动的方式，可以通过电机控制驱动线线长变化，从而使机器人到达预定位置。然而，控制过程中因传动误差、驱动线的张紧程度不同及控制模型不准确带来的误差等因素的影响，导致机器人位置控制误差较大，为了实现连续型机器人对所规划路径的跟随，前提是能控制机器人准确到达设定的目标位置。但目前尚缺少能够对连续型机器人的位置进行高精度控制的姿态控制系统。

技术实现要素：

为了解决上述问题，本发明的目的在于提供一种连续型机器人姿态控制系统。

为了达到上述目的，本发明提供的连续型机器人姿态控制系统包括核心控制器、蛇臂控制模块、伸缩机构控制模块、蛇臂姿态采集模块、驱动线拉力采集模块、微处理器、姿态传感器和压力传感器；其中核心控制器上带有液晶显示器、鼠标和键盘，与蛇臂姿态采集模块、驱动线拉力采集模块和微处理器相连接；蛇臂控制模块连接在微处理器和连续型机器人上的蛇臂电机之间，用于控制驱动连续型机器人运动的蛇臂电机的速度和位置，从而改变驱动线长度，进而改变蛇臂姿态；伸缩机构控制模块连接在微处理器和连续型机器人上的直线模组电机之间，用于控制直线模组电机的速度和位置，进而驱动连续型机器人沿直线模组进行一维运动；蛇臂姿态采集模块与安装在蛇臂每段关节末端处的姿态传感器相连接，用于将姿态传感器检测的姿态角数据通过串口传输至核心控制器，以形成姿态反馈的闭环控制；驱动线拉力采集模块与安装在蛇臂每个关节段末端圆盘上的压力传感器相连接，用于采集连续型机器人上每根驱动线的拉力值，并通过串口传输至核心控制器，以形成拉力反馈的闭环控制。

所述的蛇臂姿态采集模块采用九轴惯性导航模块JY-901，其内部集成磁力计、陀螺仪和加速度计。

所述的压力传感器采用薄膜压力传感器。

本发明提供的连续型机器人姿态控制系统具有如下有益效果：

(1)结构简便，易于搭建

系统采用的姿态传感器和薄膜压力传感器体积小，重量轻，模块集成化程度高，对机器人蛇臂部分形变的影响小。

(2)位置控制精度高、可靠性好

系统既包含机器人蛇臂的姿态反馈，还包括驱动线力的反馈，具有位置控制精度高，可靠性好的特点。

附图说明

图1为连续型机器人单关节段运动学模型。

图2为本发明提供的连续型机器人姿态控制系统构成框图。

图3为蛇臂单关节段姿态和驱动力反馈控制图。

具体实施方式

下面结合附图和具体实施例对本发明提供的连续型机器人运动控制系统进行详细说明。

1.1运动学分析

1.1.1单关节段运动学模型

对于线驱动连续型机器人，通过对蛇臂单关节模型的分析，如图1所示，可以得到单关节基座坐标系至末端坐标系的齐次变换矩阵，如下式：

式中，s为正弦函数sin的缩写，c为余弦函数cos的缩写，θ∈(0,π]表示关节段的弯曲角度，表示关节段的旋转角度，L为机器人单关节段长度。如此，通过齐次变化矩阵，建立了关节变量(弯曲角度和旋转角度)与关节末端点位置之间的映射关系。

1.1.2多关节段运动学模型

将蛇臂机构描述为多个连续型单关节的串联，假设j#关节坐标系相对于(j-1)#关节坐标系的齐次变换矩阵为那么，关节末端坐标系{N}相对于系统坐标系的齐次变换矩阵可以表示为公式：其中，

θ_j为第j#关节的弯曲角度，为第j#关节的旋转角度。如此，建立了多关节的关节变量与关节端点位置矢量之间的映射关系，即为多关节的运动学模型。

1.2本发明提供的连续型机器人姿态控制系统整体设计

如图2所示，本发明提供的连续型机器人姿态控制系统包括核心控制器1、蛇臂控制模块2、伸缩机构控制模块3、蛇臂姿态采集模块4、驱动线拉力采集模块5、微处理器6、姿态传感器7和压力传感器8；其中核心控制器1上带有液晶显示器、鼠标和键盘，与蛇臂姿态采集模块4、驱动线拉力采集模块5和微处理器6相连接；蛇臂控制模块2连接在微处理器6和连续型机器人上的蛇臂电机9之间，用于控制驱动连续型机器人运动的蛇臂电机9的速度和位置，从而改变驱动线长度，进而改变蛇臂姿态；伸缩机构控制模块3连接在微处理器6和连续型机器人上的直线模组电机10之间，用于控制直线模组电机10的速度和位置，进而驱动连续型机器人沿直线模组进行一维运动；蛇臂姿态采集模块4与安装在蛇臂每段关节末端处的姿态传感器7相连接，用于将姿态传感器7检测的姿态角数据通过串口传输至核心控制器1，以形成姿态反馈的闭环控制；驱动线拉力采集模块5与安装在蛇臂每个关节段末端圆盘上的压力传感器8相连接，用于采集连续型机器人上每根驱动线的拉力值，并通过串口传输至核心控制器1，以形成拉力反馈的闭环控制。

所述的蛇臂姿态采集模块4采用九轴惯性导航模块JY-901，其内部集成磁力计、陀螺仪和加速度计。

所述的压力传感器8采用薄膜压力传感器。这是因为采用卷绕方式控制驱动线线长变化时，驱动线是卷绕在卷轴上，因此很难在卷轴侧安装测量拉力的压力传感器，而驱动线的另一端是固定在蛇臂每个关节段的末端圆盘上，因此可在该处加装压力传感器8。

现将本发明提供的连续型机器人姿态控制系统工作原理阐述如下：首先由核心控制器1接受位置指令，经过算法解算后将三维位置量转换为蛇臂的关节变量(即旋转角度和弯曲角度)，然后通过串口发送给微处理器6；微处理器6将关节变量解算为绳长变化量和上升距离进而转化为电机的转动行程，分别发送给蛇臂控制模块2和伸缩机构控制模块3；由蛇臂控制模块2和伸缩机构控制模块3分别驱动蛇臂电机9和直线模组电机10的运动，最终驱动机器人蛇臂到达目标点。在运动过程中本姿态控制系统实时采集蛇臂的状态，其中蛇臂姿态采集模块4通过姿态传感器7采集机器人蛇臂每段关节段末端处的姿态角数据，然后传送给核心控制器1，核心控制器1经过算法解算后便可以得到各个关节段的弯曲角度和旋转角度，然后得到与输入的关节变量之间的差值。将此关节变量差值解算为绳长变化量，然后实时发出调整指令来实现蛇臂姿态快速准确调节。与此同时，驱动线拉力采集模块4通过压力传感器8实时采集每一个蛇臂关节段末端三根驱动线的拉力值，然后传送给核心控制器1，核心控制器1设定拉力阈值，并根据拉力的从小到大做出不同输出。其中在很小的拉力下输出零，代表不干预当前进程；在可控拉力范围内输出调整信号；当拉力超过最大设定值时，发出系统中断信号，停止运行并进行报警提示。这样通过蛇臂姿态反馈和驱动线拉力反馈，机器人就能在快速准确到达设定位置的前提下同时保证驱动线处于安全拉力范围以内。

1.3基于姿态和驱动力反馈的机器人控制算法设计

1.3.1末端姿态角求解

在对连续型机器人单关节段进行建模时，末端坐标系相对于首端坐标系的旋转变换为按照Z-Y-Z次序，而姿态传感器测量的姿态角为RPY角，固结在姿态传感器7上的坐标系相对于初始位置坐标系的变换按照Z-Y-X旋转次序，不同欧拉变换表示坐标系旋转变换时的欧拉角是不同的，因此，无法直接使用姿态传感器7测量的RPY角进行控制。对机器人关节段进行闭环控制需要已知当前的关节变量，故需将测量得到的ZYX欧拉角转换为ZYZ欧拉角。

由于按照不同的旋转次序均可实现坐标系的空间变换，故旋转变换矩阵相同，可利用姿态传感器测量的ZYX欧拉角求出变换矩阵，进而求解ZYZ欧拉角。

设坐标系变换的ZYX欧拉角为(ω,ξ,ψ)，则与RPY角对应关系为：

可求得旋转变换矩阵为：

设坐标系变换的ZYZ欧拉角为(α,β,γ)，则旋转变换矩阵为：

令R(α,β,γ)＝R(ω,ξ,ψ)，可求出ZYZ欧拉角(α,β,γ)。为简便表示，将R(ω,ξ,ψ)写作：

当sβ≠0时，可求出ZYZ欧拉角(α,β,γ)为：

式中，A tan2(y,x)为双变量反正切函数，是计算arctan(y/x)，并能根据x和y的符号判断角度所在的象限。

当β＝0°时，可求出ZYZ欧拉角(α,β,γ)为：

当β＝180°时，可求出ZYZ欧拉角(α,β,γ)为：

末端坐标系相对于首端坐标系绕ZYZ轴的旋转角度依次为故应满足下式：

理论上计算出的角度应满足α＝-γ

在连续型机器人姿态控制中使用(β,-γ)作为反馈的关节变量

1.3.2蛇臂姿态和驱动力反馈设计

基于姿态和驱动力反馈的蛇臂运动闭环控制器控制框图如图3所示。以机器人蛇臂关节变量作为系统的输入，姿态传感器7实时测量机器人蛇臂末端的姿态角数据，并通过蛇臂姿态采集模块4传送给核心控制器1，核心控制器1经过欧拉角变换转换为ZYZ欧拉角，可得当前蛇臂的关节变量(β,-γ)，求解关节变量的差值并计算驱动线线长变化量。在核心控制器1中设计机器人姿态PID控制器，以使机器人蛇臂姿态调节快速、准确；同时设计驱动线拉力限幅滤波器和驱动线拉力PID控制器，设定拉力最小值F_min、拉力过大调节值F_high和最大值F_max，驱动线拉力F处于不同范围时，驱动线拉力限幅滤波器给出相应输出ΔF。当压力传感器7实时检测的驱动线拉力F＜F_min或F_high≤F≤F_max时，驱动线拉力限幅滤波器有输出，通过驱动线拉力PID控制器进行调节；当F_min≤F≤F_high时，拉力处于合适的范围内，驱动线拉力限幅滤波器输出为0，不进行调整；当拉力F＞F_max时，驱动线拉力限幅滤波器不输出，系统中断运行，进行报警提示。通过蛇臂姿态和驱动线拉力反馈，实现蛇臂快速准确到达设定的姿态，并且运行中驱动线始终处于合适的拉力范围内。