CN105058398B - 一种球形轮移动机器人及其测速方法 - Google Patents

Abstract

本发明提供一种球形轮移动机器人及其测速方法，该球形轮移动机器人包括：球形轮、根据控制信号驱动球形轮运动的驱动机构、承载各结构的机身、控制模块以及获取机身姿态数据的惯性传感器；还包括：用于获取球形轮相对于机身速度的测速机构；所述控制模块根据测速机构获取的球形轮相对于机身速度和惯性传感器获取的机身姿态数据利用基于运动学约束关系的线性方程得到球形轮的求解速度ω，然后利用球形轮的求解速度和传感器的机身姿态数据结合运动控制律求得球形轮移动机器人运动的控制信号，并将该控制信号发送至驱动机构。本发明提高了机器人行走时球形轮球速的测量精度和可靠性。

Description

一种球形轮移动机器人及其测速方法

技术领域

本发明属于自动控制技术领域，尤其涉及一种球形轮移动机器人及其测速方法。

背景技术

2014年的专利——球形轮移动机器人的测速方法和控制方法，申请号201410345088.0，对传统球形轮移动机器人的机械结构进行了改进，并设计了复杂环境下的运动控制方法。

该专利中的球形轮移动机器人是通过万向驱动轮的驱动电机编码器数据解算球形轮的速度。由于驱动轮的作用是主动驱动，在驱动过程中，球形轮处于被动状态，驱动轮处于主动状态。当驱动轮突然受到一个大扭矩驱动指令而快速响应时，球形轮因为受到较大阻力或者因为惯性来不及响应驱动轮的作用，驱动轮与球形轮之间就会存在相对滑动。此时驱动电机的编码器数据就不能很好地反映球形轮的速度，从而使得球速的测量存在较大误差，进而影响控制效果，甚至使得机器人失稳。

发明内容

为解决上述问题，本发明提供一种球形轮移动机器人及其测速方法，提高了机器人行走时球形轮球速的测量精度和可靠性。

本发明的球形轮移动机器人，其包括：球形轮、根据控制信号驱动球形轮运动的驱动机构、承载各结构的机身、控制模块以及获取机身姿态数据的惯性传感器；其特征在于，还包括：

用于获取球形轮相对于机身速度的测速机构；

所述控制模块根据测速机构获取的球形轮相对于机身速度和惯性传感器获取的机身姿态数据利用基于运动学约束关系的线性方程得到球形轮的求解速度ω，然后利用球形轮的求解速度和传感器的机身姿态数据结合运动控制律求得球形轮移动机器人运动的控制信号，并将该控制信号发送至驱动机构；

其中：测速机构的数量为三个，每个测速机构均包括：固定座、弧形支架、编码器固定件、全向测速轮以及编码器；

固定座与机身的底盘固定，固定座与弧形支架铰接，编码器固定件与编码器固定，弧形支架与编码器固定件铰接，且该铰链的轴的中心线通过全向测速轮的中心；弧形支架将全向测速轮压向球形轮，使得全向测速轮与球形轮紧密贴合，全向测速轮与编码器输出轴同轴固连。

进一步的，弧形支架依次包括水平段、大圆弧段、小圆弧段三段，水平段与固定座连接，连接方式为活动铰链，活动铰链的转轴上装有扭矩弹簧，该扭矩弹簧产生的扭矩将弧形支架压向球形轮；大圆弧段所在圆弧与球形轮球心共圆心；小圆弧段的圆心在大圆弧段的弧面中心线上，小圆弧段与全向测速轮共圆心；小圆弧段通过铰链与编码器固定件连接。

进一步的，大圆弧段采用可伸缩结构，从而改变全向测速轮与球形轮的接触位置；小圆弧段通过铰链与编码器固定件连接，从而改变全向测速轮对球形轮的有效测速方向。

进一步的，编码器固定件包括圆环部分和直角部分，直角部分内表面与弧形支架小圆弧段外表面通过铰链连接，该铰链的轴的中心线通过全向测速轮的中心；圆环部分的环面上设有与编码器螺孔匹配的螺孔，编码器通过螺孔与编码器固定件的环形连接部分同轴心固连。

进一步的，驱动机构的数量为三个，对称固定于机身的底盘上，每个驱动机构包括：驱动电机编码器、驱动电机、万向驱动轮；驱动电机输出轴一端同轴安装万向驱动轮，另一端安装驱动电机编码器；驱动电机与机身底盘固定；万向驱动轮与球形轮相切。

进一步的，设角度γ为连线与球形轮的垂线之间的夹角，所述连线为全向测速轮的中心与球形轮的球心的连线；该角度γ为90°；

设夹角β为全向测速轮所在平面与球形轮经线所在平面的夹角，该夹角β为45°。

进一步的，获得球形轮移动机器人运动的控制信号的步骤具体包括：

步骤1，控制模块分别解算每组测速机构的编码器的数据n₁₁、n₁₂、n₁₃，利用公式计算出球形轮的相对于机身的第一角速度ω_r1＝[ω_1x ω_1y ω_1z]；表示测速编码器转速与测量点线速度之间的比例系数；r_w表示万向测量轮的半径；

上式中所有矢量均处于球形轮坐标系中：

v₁₁、v₁₂、v₁₃分别表示三个测速机构与球轮接触点处的线速度矢量，且p′₁₁、p′₁₂、p′₁₃分别表示三个测速机构与球轮接触点处的实时位置矢量，

分别表示三个测速机构在球形轮移动机器人整体处于铅垂状态时的测量点位置矢量，夹角γ预先设定，R表示球轮的半径；M_ψθφ为惯性传感器测得的三个测速机构相对于铅垂状态的旋转矩阵；

分别表示三个测速机构的全向测速轮在球形轮坐标系的夹角β下测得的有效测速方向矢量，预先设定；

步骤2，控制模块分别解算每个驱动机构的驱动电机编码器的数据n₂₁、n₂₂、n₂₃，利用公式计算出球形轮相对于机身的第二角速度ω_r2＝[ω_2x ω_2y ω_2z]；表示驱动电机编码器测得的转速度与接触点处线速度的比例系数；r_d表示万向驱动轮的半径；

上式中所有矢量均处于球形轮坐标系中：

v₂₁、v₂₂、v₂₃分别表示三个驱动机构与球轮接触点处的线速度矢量，且p′₂₁、p′₂₂、p′₂₃分别表示三个驱动机构与球轮接触点处的实时位置矢量，分别表示三个驱动机构在球形轮移动机器人整体处于铅垂状态时的位置矢量；R_ψθφ为惯性传感器测得的三个驱动机构相对于铅垂状态的旋转矩阵；

分别表示三个驱动机构的万向驱动轮在球形轮坐标系下的有效测速方向矢量；

步骤3，控制模块对第一角速度ω_r1和第二角速度ω_r2进行数据融合，利用滤波公式ω_r＝ω_r2+k(ω_r1-ω_r2)，计算出球形轮的相对速度ω_r；然后将球形轮的相对速度ω_r与惯性传感器的姿态数据中的陀螺数据ω_gyr结合，得到球形轮的解算速度ω＝ω_r+ω_gyr；其中，k为卡尔曼滤波系数；

步骤4，利用解算速度ω和惯性传感器的数据在周期T内计算控制量，该控制量即为球形轮移动机器人运动的控制信号。

有益效果：

(1)本发明设计的球轮测速机构，能够紧贴球形轮表面；

(2)全向测速轮能够将测速点的线速度分解为测量速度和自由速度，在有效测速的同时不影响球轮的运动；

(3)测速轮的测量位置和测速方位可调，能够方便、准确地测量球形轮表面(除与地面接触和球形轮顶部附近区域外)任意位置、任意方位的线速度。

(4)本发明设计的三组对称球轮测速机构，能够快速、精确地测量球形轮的转动速度，解算速度快、测量精度高、可靠性高；

(5)本发明设计的测速算法，能够在球轮低速和高速时均有效、准确地测量球速，对以本发明为基础运动平台在复杂环境下的应用打下了坚实的基础。

附图说明

图1为本发明的结构示意图；

图2为本发明中球形轮测速机构的主视图；

图3为本发明中球形轮测速机构的单体侧视图；

图4为本发明中球形轮测速机构的单体任意角视图；

图5为本发明中球形轮测速机构电机固定件的任意角视图。

具体实施方式

如图1所示，球形轮移动机器人包括：球形轮1、三个相同的驱动机构2、机身3、控制模块4、三个相同的测速机构5以及惯性传感器6；

驱动机构2包括：驱动电机编码器21、驱动电机22、万向驱动轮23；

控制模块4和惯性传感器6安装于机身3内；三个相同的驱动机构2对称固定于机身3底盘24上，并均与球形轮1相切，如图1中所示；

测速机构5，检测球形轮1相对于机身3的第一角速度ω_r1；

惯性传感器6，检测机身3的姿态数据，该姿态数据主要包括：相对于地面的转动速度ω_gyr和旋转矩阵R_ψθφ；

控制模块4，分析球形轮移动机器人姿态和球形轮1速度得到球形轮1的解算速度ω，根据球形轮1的解算速度及姿态数据得出控制策略输出控制信号；

驱动机构2，根据控制信号驱动球形轮1，或者检测球形轮1相对于机身3的第二角速度ω_r2；

参见图2、3、4、5，每个测速机构5的包括：固定座7、弧形支架8、编码器固定件9、全向测速轮10以及编码器11。

固定座7与机身3的底盘24固定，固定座7和弧形支架8铰接，编码器固定件9固定编码器11，弧形支架8和编码器固定件9铰接，且该铰链的轴的中心线通过全向测速轮10的中心，如附图3所示；

图3中，弧形支架8包括水平段、大圆弧段、小圆弧段三段，水平段与固定座7连接，连接方式为活动铰链，活动铰链的转轴上装有扭矩弹簧，扭矩弹簧产生的扭矩25将弧形支架8压向球形轮1，从而使得全向测速轮10与球形轮1紧贴；水平段之后的过渡连接段是大圆弧段，大圆弧段所在圆弧与球形轮1的经线共圆心，大圆弧部分采用可伸缩结构，大圆弧的长度可调，即可改变全向测速轮10与球形轮1的接触位置，该位置由图2中的角度13决定，角度13为连线与球形轮1的垂线之间的夹角，所述连线为全向测速轮10的中心与球形轮1的球心的连线；大圆弧段连接小圆弧段，且小圆弧的圆心在大圆弧的弧面中心线上，小圆弧与全向测速轮10共圆心；小圆弧段通过铰链与编码器固定件9连接。

编码器固定件9如图5所示，包括圆环部分和直角部分。在附图4中，编码器固定件9的直角部分与弧形支架8通过铰链连接，编码器固定件9直角部分内表面与弧形支架8的小圆弧段外表面铰接，铰链的轴的中心线通过全向测速轮10的中心，如附图3所示；铰链的轴为蝶形螺栓，加紧蝶形螺栓可以将铰链变为固定铰链，使全向测速轮10所在平面与球形轮经线所在平面的夹角12固定，如附图2所示；编码器固定件9的环形连接部分有螺孔与编码器11的螺孔匹配，编码器10通过螺孔与编码器固定件9的环形连接部分同轴心固连，如附图4所示。

全向测速轮10与编码器11输出轴同轴固连，全向测速轮10外缘的滚轮保持与球形轮1的表面相切；全向测速轮10的外边缘的滚轮可绕自身轴自由转动，并且同时随全向测速轮10转动；滚轮将球形轮接触点处的线速度分解为绕自身转轴的自由速度和绕编码器输出轴的测量速度。

驱动机构2包括：驱动电机编码器21、驱动电机22、万向驱动轮23；驱动电机22输出轴一端同轴安装万向驱动轮23，另一端安装驱动电机编码器21；驱动电机22与机身3底盘24固定。

本申请中的同轴心设计，以及角度13γ(图3中)和夹角12β(图2中)的可调设计，使得全向测速轮10可以被安装在球形轮1表面的任意位置，全向测速轮11可以以任意角度与球形轮1表面垂直相切。

本申请中，选取角度13γ＝90°(图3中)，夹角12β＝45°(图2中)，并采用三组对称测速机构5。这种结构能有效测量球形轮的转速，且使得球形轮速度的解算变得简单，计算耗时少，安装更加容易；同时，比采用两组测速机构5的可靠性高、测量精度高，比采用三组以上的测速机构5的冗余度低，成本更低。

控制模块4根据接收到的惯性传感器6的姿态数据进行姿态解算，根据测速机构5和驱动机构2的数据进行数据融合处理，解算出球速，然后利用姿态数据和球速执行运动控制算法，计算出对驱动电机的控制量，并依此对驱动机构2进行驱动；

本实例的工作流程为：

系统启动后，惯性传感器6实时采集姿态数据，三组测速机构5驱动电机编码器21分别实时测量驱动电机22转速；控制模块4读取惯性传感器6、测速编码器11和驱动电机编码器21的数据；解算机器人的姿态和球速，根据解算结果执行运动控制策略；计算驱动电机的控制量并发送给驱动机构2；在驱动轮23的驱动下，球形轮1实时调节转速，最终实现机器人的姿态稳定和运动控制。

具体的包括以下步骤：

步骤1，控制模块(4)分别解算每组测速机构(5)的编码器(11)的数据n₁₁、n₁₂、n₁₃，利用公式计算出球形轮(1)的相对于机身(3)的第一角速度ω_r1＝[ω_1xω_1y ω_1z]；表示测速编码器转速与测量点线速度之间的比例系数；r_w表示万向测量轮(10)的半径；

上式中所有矢量均处于球形轮坐标系中：

v₁₁、v₁₂、v₁₃分别表示三个测速机构(5)与球轮(1)接触点处的线速度矢量，且p′₁₁、p′₁₂、p′₁₃分别表示三个测速机构(5)与球轮(1)接触点处的实时位置矢量，

分别表示三个测速机构(5)在球形轮移动机器人整体处于铅垂状态时的测量点位置矢量，夹角γ预先设定，R表示球轮的半径；M_ψθφ为惯性传感器(6)测得的三个测速机构(5)相对于铅垂状态的旋转矩阵；

分别表示三个测速机构(5)的全向测速轮(10)在球形轮坐标系的夹角β下测得的有效测速方向矢量，预先设定；

步骤2，控制模块(4)分别解算每个驱动机构(2)的驱动电机编码器(21)的数据n₂₁、n₂₂、n₂₃，利用公式计算出球形轮(1)相对于机身(3)的第二角速度ω_r2＝[ω_2x ω_2y ω_2z]；表示驱动电机编码器测得的转速度与接触点处线速度的比例系数；r_d表示万向驱动轮(23)的半径；

上式中所有矢量均处于球形轮坐标系中：

v₂₁、v₂₂、v₂₃分别表示三个驱动机构(2)与球轮(1)接触点处的线速度矢量，且p′₂₁、p′₂₂、p′₂₃分别表示三个驱动机构(2)与球轮(1)接触点处的实时位置矢量，分别表示三个驱动机构(2)在球形轮移动机器人整体处于铅垂状态时的位置矢量；R_ψθφ为惯性传感器(6)测得的三个驱动机构(2)相对于铅垂状态的旋转矩阵；

分别表示三个驱动机构(2)的万向驱动轮(23)在球形轮坐标系下的有效测速方向矢量；

步骤3，控制模块(4)对第一角速度ω_r1和第二角速度ω_r2进行数据融合，利用滤波公式ω_r＝ω_r2+k(ω_r1-ω_r2)，计算出球形轮的相对速度ω_r；然后将球形轮的相对速度ω_r与惯性传感器(6)的姿态数据中的陀螺数据ω_gyr结合，得到球形轮(1)的解算速度ω＝ω_r+ω_gyr；其中，k为卡尔曼滤波系数；k为卡尔曼滤波系数，由于测速机构比驱动机构的测速精度更高，经过融合之后，能在正常行进中获得更高的测速精度，在打滑状态测得有效数据。

步骤4，利用解算速度ω和惯性传感器(6)的数据在周期T内计算控制量，该控制量即为球形轮移动机器人运动的控制信号。

当然，本发明还可有其他多种实施例，在不背离本发明精神及其实质的情况下，熟悉本领域的技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (6)

1.一种球形轮移动机器人，其包括：球形轮(1)、根据控制信号驱动球形轮(1)运动的驱动机构(2)、承载各结构的机身(3)、控制模块(4)以及获取机身姿态数据的惯性传感器(6)；其特征在于，还包括：

用于获取球形轮(1)相对于机身(3)速度的测速机构(5)；

所述控制模块(4)根据测速机构(5)获取的球形轮(1)相对于机身(3)速度和惯性传感器(6)获取的机身姿态数据，利用基于运动学约束关系的线性方程得到球形轮(1)的解算速度ω，然后利用球形轮(1)的解算速度和传感器(6)的机身姿态数据结合运动控制律求得球形轮移动机器人运动的控制信号，并将该控制信号发送至驱动机构(2)；

其中：测速机构(5)的数量为三个，每个测速机构(5)均包括：固定座(7)、弧形支架(8)、编码器固定件(9)、全向测速轮(10)以及编码器(11)；

固定座(7)与机身(3)的底盘(24)固定，固定座(7)与弧形支架(8)铰接，编码器固定件(9)与编码器(11)固定，弧形支架(8)与编码器固定件(9)铰接，且铰链的轴的中心线通过全向测速轮(10)的中心；弧形支架(8)将全向测速轮(10)压向球形轮(1)，使得全向测速轮(10)与球形轮(1)紧密贴合，全向测速轮(10)与编码器(11)输出轴同轴固连；

获得球形轮移动机器人运动的控制信号的步骤具体包括：

步骤1，控制模块(4)分别解算每组测速机构(5)的编码器(11)的数据n₁₁、n₁₂、n₁₃，利用公式计算出球形轮(1)的相对于机身(3)的第一角速度ω_r1＝[ω_1x ω_1yω_1z]；表示测速编码器转速与测量点线速度之间的比例系数；r_w表示万向测量轮(10)的半径；

上式中所有矢量均处于球形轮坐标系中：

v₁₁、v₁₂、v₁₃分别表示三个测速机构(5)与球轮(1)接触点处的线速度矢量，且p'₁₁、p'₁₂、p'₁₃分别表示三个测速机构(5)与球轮(1)接触点处的实时位置矢量，

分别表示三个测速机构(5)在球形轮移动机器人整体处于铅垂状态时的测量点位置矢量，夹角γ(13)预先设定，R表示球轮的半径；M_ψθφ为惯性传感器(6)测得的三个测速机构(5)相对于铅垂状态的旋转矩阵；

分别表示三个测速机构(5)的全向测速轮(10)在球形轮坐标系的夹角β(12)下测得的有效测速方向矢量，预先设定；

步骤2，控制模块(4)分别解算每个驱动机构(2)的驱动电机编码器(21)的数据n₂₁、n₂₂、n₂₃,利用公式计算出球形轮(1)相对于机身(3)的第二角速度ω_r2＝[ω_2xω_2y ω_2z]；表示驱动电机编码器测得的转速度与接触点处线速度的比例系数；r_d表示万向驱动轮(23)的半径；

上式中所有矢量均处于球形轮坐标系中：

v₂₁、v₂₂、v₂₃分别表示三个驱动机构(2)与球轮(1)接触点处的线速度矢量，且p'₂₁、p'₂₂、p'₂₃分别表示三个驱动机构(2)与球轮(1)接触点处的实时位置矢量，分别表示三个驱动机构(2)在球形轮移动机器人整体处于铅垂状态时的位置矢量；R_ψθφ为惯性传感器(6)测得的三个驱动机构(2)相对于铅垂状态的旋转矩阵；

分别表示三个驱动机构(2)的万向驱动轮(23)在球形轮坐标系下的有效测速方向矢量；

步骤3，控制模块(4)对第一角速度ω_r1和第二角速度ω_r2进行数据融合，利用滤波公式ω_r＝ω_r2+k(ω_r1-ω_r2)，计算出球形轮的相对速度ω_r；然后将球形轮的相对速度ω_r与惯性传感器(6)的姿态数据中的陀螺数据ω_gyr结合，得到球形轮(1)的解算速度ω＝ω_r+ω_gyr；其中，k为卡尔曼滤波系数；

步骤4，利用解算速度ω和惯性传感器(6)的数据在周期T内计算控制量，该控制量即为球形轮移动机器人运动的控制信号。

2.如权利要求1所述的球形轮移动机器人，其特征在于，弧形支架(8)依次包括水平段、大圆弧段、小圆弧段三段，水平段与固定座(7)连接，连接方式为活动铰链，活动铰链的转轴上装有扭矩弹簧，该扭矩弹簧产生的扭矩(25)将弧形支架(8)压向球形轮(1)；大圆弧段所在圆弧与球形轮(1)球心共圆心；小圆弧段的圆心在大圆弧段的弧面中心线上，小圆弧段与全向测速轮(10)共圆心；小圆弧段通过铰链与编码器固定件(9)连接。

3.如权利要求2所述的球形轮移动机器人，其特征在于，

大圆弧段采用可伸缩结构，从而改变全向测速轮(10)与球形轮(1)的接触位置；小圆弧段通过铰链与编码器固定件(9)连接，从而改变全向测速轮(10)对球形轮(1)的有效测速方向。

4.如权利要求1所述的球形轮移动机器人，其特征在于，编码器固定件(9)包括圆环部分和直角部分，直角部分内表面与弧形支架(8)小圆弧段外表面通过铰链连接，该铰链的轴的中心线通过全向测速轮(10)的中心；圆环部分的环面上设有与编码器(11)螺孔匹配的螺孔，编码器(11)通过螺孔与编码器固定件(9)的环形连接部分同轴心固连。

5.如权利要求1所述的球形轮移动机器人，其特征在于，驱动机构(2)的数量为三个，对称固定于机身(3)的底盘(24)上，每个驱动机构(2)包括：驱动电机编码器(21)、驱动电机(22)、万向驱动轮(23)；驱动电机(22)输出轴一端同轴安装万向驱动轮(23)，另一端安装驱动电机编码器(21)；驱动电机(22)与机身(3)底盘(24)固定；万向驱动轮(23)与球形轮(1)相切。

6.如权利要求1所述的球形轮移动机器人，其特征在于，

设角度γ(13)为连线与球形轮(1)的垂线之间的夹角，所述连线为全向测速轮(10)的中心与球形轮(1)的球心的连线；该角度γ(13)为90°；

设夹角β(12)为全向测速轮(10)所在平面与球形轮(1)经线所在平面的夹角，该夹角β(12)为45°。