[发明专利]移动机器人视觉系统防抖装置与防抖补偿控制方法

权利要求书

1.一种移动机器人视觉系统防抖装置，所述视觉系统防抖装置由防抖机 构、传感器系统和防抖控制系统三部分构成；其特征在于：所述防抖机构由左 右移动单元、上下移动单元、俯仰单元、偏摆单元和基座(22)构成，上下移 动单元、左右移动单元、俯仰单元和偏摆单元均设置在基座(22)上；所述传 感器系统包括倾角传感器(26)、第一振动传感器(24)和第二振动传感器(25)， 倾角传感器(26)、第一振动传感器(24)和第二振动传感器(25)均安装在 基座(22)上；所述防抖控制系统主要包括控制系统硬件和控制系统软件，用 于视觉系统的防抖；

上下移动单元由上下驱动电机(1)、上下移动单元传动机构、上下运动导 向轴(7)、固定块(8)、滑块(6)和上下运动同步带(28)构成；上下移动 单元传动机构包括上下运动绳轮(2)、上下运动换向轮(3)、第一上下运动同 步带轮(4)和第二上下运动同步带轮(5)；上下驱动电机(1)通过上下移动 单元传动机构驱动与上下运动同步带(28)固连的固定块(8)及滑块(6)一 起沿上下运动导向轴(7)上下运动；上下移动单元设置在安装有两个视觉传 感器(27)的架体(50)的下方，滑块(6)与架体(50)连接以实现两个视 觉传感器(27)相对基座(22)同时上下移动；

左右移动单元由左右驱动电机(18)、左右移动单元传动机构和左右运动 导向轴(21)构成；左右移动单元传动机构包括第一左右同步带轮(19)、第 二左右同步带轮(20)和左右运动同步带(29)；左右驱动电机(18)通过左 右移动单元传动机构驱动与左右运动同步带(29)固连的滑块(6)一起沿着 导向轴(21)左右运动；左右移动单元设置在安装有两个视觉传感器(27)的 架体(50)的下方，滑块(6)与架体(50)连接以实现两个视觉传感器(27) 相对基座(22)同时左右移动；

俯仰单元由俯仰驱动电机(14)、俯仰单元传动机构和支架(17)构成， 俯仰单元传动机构包括第一俯仰绳轮(15)和第二俯仰绳轮(16)；俯仰驱动 电机(14)通过俯仰单元传动机构驱动支架(17)做俯仰运动，固装在支架(17) 内的两个视觉传感器(27)相对基座(22)同时做俯仰运动，固装在支架(17) 内的两个视觉传感器(27)连同支架(17)安装在架体(50)内；

偏摆单元由两个偏摆驱动电机(9)和两套偏摆单元传动机构构成；每套 偏摆单元传动机构包括第一偏摆绳轮(10)、偏摆换向轮(11)、第一偏摆同步 带轮(12)和第二偏摆同步带轮(13)；两个偏摆驱动电机(9)、两套偏摆单 元传动机构左右对称设置，每个偏摆驱动电机(9)通过相应的偏摆单元传动 机构实现每个视觉传感器(27)相对基座(22)偏摆运动；

所述控制系统硬件由主控器(51)和五个电机驱动与控制器(52)组成； 五个电机驱动与控制器(52)分别控制上下驱动电机(1)、左右驱动电机(18)、 俯仰驱动电机(14)以及两个偏摆驱动电机(9)的运动；控制系统软件是基 于移动机器人视觉系统防抖装置的防抖补偿控制方法利用相应程序语言编写 的专业软件；传感器系统将感知到的移动机器人视觉系统抖动信息输出给主控 器(51)进行处理，主控器(51)内的控制系统软件进行补偿计算得到相应补 偿量后输出相应的命令信息给五个电机驱动与控制器(52)，五个电机驱动与 控制器(52)控制、驱动防抖机构中相应的电机运转进行抖动补偿，以控制两 个视觉传感器(27)相对地面保持平稳，从而实现移动机器人视觉系统的防抖 目的。

2.一种权利要求1所述移动机器人视觉系统防抖装置的防抖补偿控制方 法，其特征在于：所述方法按照以下步骤实现的：

步骤一、由第一振动传感器(24)、第二振动传感器(25)和倾角传感器 (26)测出来的抖动参数h₁、h₂、及相应的差分量确定测得抖动量变化 的速度和加速度量；

h₁表示基座(22)沿Z轴方向的移动；

h₂表示基座(22)沿Y轴方向的移动；

表示基座(22)相对X轴方向转角；

表示基座(22)相对Y轴方向转角；

其中，X轴、Y轴所在坐标系的坐标原点位于基座的几何中心处；

步骤二、根据传感器系统在采样时间T内测量的情况，判断移动机器人视 觉系统是否有抖动；如果有抖动需进行抖动补偿，执行步骤三、四、五；否则 则不需补偿；

步骤三、计算补偿量：任意构形{a₁，a₂，θ₃，θ₄，θ₅}下两个视觉传感器(27) 相对于装置基座(22)的位姿矩阵分别为⁰T_CCD4＝⁰A₁¹A₂²A₃³A₄、 ⁰T_CCD5＝⁰A₁¹A₂²A₃³A₅；当前构形下两个视觉传感器(27)相对于 装置基座(22)的位姿矩阵分别为由 第一振动传感器(24)、第二振动传感器(25)、倾角传感器(26)测出来 的抖动参数h₁、h₂、确定视觉传感器(27)相对于装置基座(22) 抖动的位姿矩阵为则有如下关系 式：

上述式中的X、Y表示X轴、Y轴，X轴、Y轴所在坐标系的坐标原点位 于基座的几何中心处；

由上述两个式子在检测得到抖动参数h₁、h₂、的情况下可推算出 防抖机构在当前构形过度到抖动补偿构形{a₁，a₂，θ₃，θ₄，θ₅}情况 下须补偿的各关节抖动补偿矢量(a1-a^1,a2-a^2,θ3-θ^3,θ4-θ^4),(a1-a^1,]]>a2-a^2,θ3-θ^3,θ5-θ^5),]]>即两个视觉传感器(27)在理论上的抖动补偿矢 量分别为Δq=(Δq1,Δq2,Δq3,Δq4)=(a1-a^1,a2-a^2,θ3-θ^3,θ4-θ^4),]]>Δq=(Δq1,Δq2,Δq3,Δq4)=(a1-a^1,a2-a^2,θ3-θ^3,θ5-θ^5),]]>进行微分后可得则防抖装置各关节补偿后位置矢量为：

a₁表示两个视觉传感器(27)的左右移动矢量；

a₂表示两个CCD的上下移动矢量；

θ₃表示两个CCD的俯仰角；

θ₄表示CCD1的偏摆角；

θ₅表示CCD2的偏摆角；

⁰A₁表示左右移动单元相对于基座(22)的位姿矩阵；

¹A₂表示上下移动单元相对于左右移动单元的位姿矩阵；

²A₃表示俯仰单元相对于上下移动单元的位姿矩阵；

³A₄表示偏摆单元中CCD1相对于俯仰单元的位姿矩阵；

³A₅表示偏摆单元中CCD2相对于俯仰单元的位姿矩阵；

步骤四、规划补偿轨迹：Δq_i(t)，t∈(0，T)，求得关节抖动补偿矢量Δq后， 为了保证在采样时间T内能完成补偿过程，即关节抖动补偿矢量从0到Δq， 而且在T时刻时各关节的速度和加减速度都为0，必须在采样时间T内对补 偿的过程进行4-3-4关节轨迹插值规划；

步骤五、基于PD反馈和逆动力学计算的抖动补偿控制：以 q=q(qd1,qd2,qd3,qd4)=q(a^1+Δq1,a^2+Δq2,θ^3+Δq3,(θ4+Δq4)]]>或(θ₅+Δq₅))及该位 置下速度q·=q·(q·d1,q·d2,q·d3,q·d4),]]>加速度q··=q··(q··d1,q··d2,q··d3,q··d4)]]>为期望的控制输 入，采用PD反馈和逆动力学计算的方法对防抖装置的各自由度运动单元上 的电机进行控制；反馈量为来自于各电机上的光电编码器位置和速度量经 机械传动装置折算到关节侧的量。