CN101612735B

CN101612735B - 移动机器人视觉系统防抖装置与防抖补偿控制方法

Info

Publication number: CN101612735B
Application number: CN2009100725857A
Authority: CN
Inventors: 吴伟国; 邵益凯
Original assignee: Harbin Institute of Technology
Current assignee: Harbin Institute of Technology
Priority date: 2009-07-24
Filing date: 2009-07-24
Publication date: 2012-03-28
Anticipated expiration: 2029-07-24
Also published as: CN101612735A

Abstract

移动机器人视觉系统防抖装置与防抖补偿控制方法，它涉及移动机器人用视觉系统防抖装置与防抖补偿控制方法。针对移动机器人的抖动情况，通过机械和控制手段从根本上实时地消除机器人视觉系统的抖动，以达到视觉系统运动的平稳。视觉系统防抖装置由防抖机构、传感器系统、防抖控制系统构成；防抖控制系统用来实现视觉系统的防抖。本方法步骤为：抖动参数的测量、判断移动机器人视觉系统是否有抖动、计算补偿量、规划补偿轨迹和基于PD反馈和逆动力学计算的抖动补偿控制。防抖装置实现对两个视觉传感器较大范围内的运动控制并对两个视觉传感器产生的抖动进行防控。采用防抖补偿控制方法后的两个视觉传感器的各项位姿参数曲线能够较为精确地接近于期望位姿参数曲线。

Description

移动机器人视觉系统防抖装置与防抖补偿控制方法

技术领域

本发明涉及机器人用视觉系统防抖装置与防抖补偿控制方法，属于机器人应用领域。

背景技术

机器人视觉是一门新兴的发展迅速的学科，八十年代以来，机器人视觉的研究已经历了从实验室走向实际应用的发展阶段。但与此同时，机器人视觉系统也出现了不少问题。机器人的移动过程中也经常会遇到路面不平等抖动现象，其视觉系统中所拍摄到的图像亦无法避免运动模糊现象。

针对运动模糊问题，通常采用较高的快门速度可以减轻这种震动所导致的照片模糊，但在曝光时间较长的拍摄中，比如在阴天或者傍晚时，拍的画面都比较虚，这是由于光线较弱的环境下而要比较长的快门时间，在这种情况下，震动所带来的图像模糊问题几乎不可避免。为了缓解这个问题，使摄影者轻松拍出清晰得照片，相机生产厂家纷纷开发了减轻防抖得技术，称为“防抖”(Anti-Shake简称AS)或图像稳定(Image Stabilization简称IS)技术。

目前，已经有一些具备防抖功能的摄像机已经被研制出来，包括以佳能、松下为首的光学防抖技术，以奥林巴斯、尼康为首的电子防抖技术，还有柯尼卡美能达独家的机身防抖技术，但它们都不是针对移动机器人用的，也不是用机械的方法去实施达到的。经文献检索发现，专利公告号为1096622，专利号为94101806，名称为防止图象抖动的摄象机。该摄象机包含检测摄象机抖动的抖动检测装置，获得校正量的计算装置和校正摄象机体的抖动的校正装置。但是该摄象机只能检测极其微小的抖动，校正装置也是通过镜头变焦来实现的，其抖动环境与移动机器人不一样，校正方式也无法适用于移动机器人。专利公告号为1610387，专利号为200410087084，名称为图像拾取设备及其照相机抖动校正方法。该发明涉及图像拾取设备及其照相机抖动校正方法，是通过图像拾取装置拾取一个对象的图像，以此来确定照相机抖动分量，从而来校正相机的抖动。该发明的不足之处是在抖动图像出来之后才开始校正的，不能达到一次性输出清晰图像的效果。另外。专利公告号为1812501，专利号为200610001684，名称为摄像装置及其摄像方法。该摄像装置是根据快门速度改变摇摄控制，从而提供适于拍摄条件的防抖动效果。该装置的适用条件是相对稳定的摇摄条件，很难适用于移动机器人所面对的复杂的无规则的抖动环境。民用摄像机和照相机内部抖动系统仅能补偿低频和小抖动量情况下的抖动，而面向轮式、履带式、腿式等移动机器人的视觉系统均属于工业视觉摄像装置，其装置内、外无抖动措施，而带有工业视觉系统的移动机器人在不平整地面或野外移动时因震动导致视觉装置抖动成像不清晰，来自地面的抖动可能是中低频，也有高频情况。而且抖动幅度有小有大，因此，上述专利中提及的防抖方法均不能解决移动机器人在不平整路面或野外环境下的复杂无规则的抖动主动补偿与控制问题。

发明内容

本发明的目的是针对移动机器人的抖动情况，通过机械和控制手段从根本上实时地消除机器人视觉系统的抖动，以达到视觉系统运动的平稳，一次性输出较清晰图像的效果；进而提供了一种移动机器人视觉系统防抖装置与防抖补偿控制方法。

本发明为达到上述发明目的所采用的技术方案包括视觉系统防抖装置和防抖补偿控制方法两部分：

本发明所述视觉系统防抖装置由防抖机构、传感器系统和防抖控制系统三部分构成；所述防抖机构由左右移动单元、上下移动单元、俯仰单元、偏摆单元和基座构成，上下移动单元、左右移动单元、俯仰单元和偏摆单元均设置在基座上；所述传感器系统包括倾角传感器、第一振动传感器和第二振动传感器，倾角传感器、第一振动传感器和第二振动传感器均安装在基座上；所述防抖控制系统主要包括控制系统硬件和控制系统软件，用来实现视觉系统的防抖；

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

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

俯仰单元由俯仰驱动电机、俯仰单元传动机构和支架构成，俯仰单元传动机构包括第一俯仰绳轮和第二俯仰绳轮；俯仰驱动电机通过俯仰单元传动机构驱动支架做俯仰运动，固装在支架内的两个视觉传感器相对基座同时做俯仰运动；

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

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

本发明所述移动机器人视觉系统防抖装置的防抖补偿控制方法，所述方法按照以下步骤实现的：

步骤一、由第一振动传感器、第二振动传感器和倾角传感器测出来的抖动参数h₁、h₂、

及相应的差分量确定测得抖动量变化的速度和加速度量；

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

步骤三、计算补偿量：任意构形{a₁，a₂，θ₃，θ₄，θ₅}下两个CCD 27相对于装置基座22的位姿矩阵分别为⁰T_CCD4＝⁰A₁ ¹A₂ ²A₃ ³A₄、⁰T_CCD5＝⁰A₁ ¹A₂ ²A₃ ³A₅；当前构形下两个视觉传感器相对于装置基座的位姿矩阵分别为

{\hat{T}}_{CCD 4}^{0} = {\hat{A}}_{1}^{0} {\hat{A}}_{2}^{1} {\hat{A}}_{3}^{2} {\hat{A}}_{4}^{3},

{\hat{T}}_{CCD 5}^{0} = {\hat{A}}_{1}^{0} {\hat{A}}_{2}^{1} {\hat{A}}_{3}^{2} {\hat{A}}_{5}^{3};

由第一振动传感器、第二振动传感器、倾角传感器测出来的抖动参数h₁、h₂、

(进行差分后可得

确定视觉传感器相对于装置基座抖动的位姿矩阵为

则有如下关系式：

由上述两个式子在检测得到抖动参数h₁、h₂、的情况下可推算出防抖机构在当前构形

过度到抖动补偿构形{a₁，a₂，θ₃，θ₄，θ₅}情况下须补偿的各关节运动量矢量

(a_{1} - {\hat{a}}_{1}, a_{2} - {\hat{a}}_{2}, θ_{3} - {\hat{θ}}_{3}, θ_{4} - {\hat{θ}}_{4}),

(a_{1} - {\hat{a}}_{1}, a_{2} - {\hat{a}}_{2}, θ_{3} - {\hat{θ}}_{3}, θ_{5} - {\hat{θ}}_{5}),

即两个视觉传感器在理论上的抖动补偿量矢量分别为

Δq = (Δ q_{1}, Δ q_{2}, Δ q_{3}, Δ q_{4}) = (a_{1} - {\hat{a}}_{1}, a_{2} - {\hat{a}}_{2}, θ_{3} - {\hat{θ}}_{3}, θ_{4} - {\hat{θ}}_{4}),

Δq = (Δ q_{1}, Δ q_{2}, Δ q_{3}, Δ q_{4}) = (a_{1} - {\hat{a}}_{1}, a_{2} - {\hat{a}}_{2}, θ_{3} - {\hat{θ}}_{3}, θ_{5} - {\hat{θ}}_{5}),

进行微分后可得

则防抖装置各关节位置矢量为：

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

步骤五、基于PD反馈和逆动力学计算的抖动补偿控制：以

及该位置下速度

\overset{\cdot}{q} = \overset{\cdot}{q} ({\overset{\cdot}{q}}_{d 1}, {\overset{\cdot}{q}}_{d 2}, {\overset{\cdot}{q}}_{d 3}, {\overset{\cdot}{q}}_{d 4}),

加速度

\overset{\cdot \cdot}{q} = \overset{\cdot \cdot}{q} ({\overset{\cdot \cdot}{q}}_{d 1}, {\overset{\cdot \cdot}{q}}_{d 2}, {\overset{\cdot \cdot}{q}}_{d 3}, {\overset{\cdot \cdot}{q}}_{d 4})

为期望的控制输入，采用PD反馈和逆动力学计算的方法对防抖装置的各自由度运动单元上的电机进行控制；反馈量为来自于各电机上的光电编码器位置和速度量经机械传动装置折算到关节侧的量。

本发明的技术效果：

本发明所述防抖装置设置左右移动单元、上下移动单元、俯仰单元、偏摆单元，具有5个自由度，实现对两个视觉传感器较大范围内的运动控制并对两个视觉传感器产生的抖动进行防控。

本发明方法可实现在复杂的无规则的抖动环境中有效控制两个视觉传感器相对地面保持平稳，从而实现移动机器人视觉系统的防抖目的，进而一次性输出较清晰图像。控制系统仿真实验结果表明(参见图9a～图9d)：在随机抖动检测情况下，采用本发明所述的防抖装置及其抖动补偿控制方法可以大幅度提高视觉传感器相对于地面的位姿精度；采用本发明所述的防抖补偿控制方法后的两个视觉传感器的各项位姿参数曲线图9d能够较为精确地接近于图9a所示的期望位姿参数曲线。

附图说明

图1a是本发明的视觉系统防抖装置的三维立体图，图1b是为上下移动单元的立体图，图1c是偏摆单元的立体图，图1d是俯仰单元的主视图，图1e是左右移动单元的立体图，图1f是本发明的视觉系统防抖装置的控制系统框图；图2是本发明的视觉系统防抖装置的机构简图；图3是本发明的视觉系统防抖装置的D-H坐标图；图4为在一个采样周期内关节补偿量的4-3-4关节插值轨迹规划曲线；图5是本发明的视觉系统防抖装置的动力学简化模型图；图6为基于PD反馈和逆动力学前馈的控制器框图；图7是本发明的视觉系统防抖装置在仿人机器人的应用示意图；图8是本发明的视觉系统防抖装置在轮式移动机器人的应用示意图；图9a为无抖动时CCD相对于地面的位姿曲线图，图9b抖动频率f＝20时随机抖动参数h₁、h₂曲线图，图9c为有抖动不补偿时CCD相对于地面的位姿曲线图，图9d进行抖动补偿时CCD相对于地面的位姿曲线图。

具体实施方式

具体实施方式一、下面结合图1a～1f、图2说明本实施方式：本实施方式所述的移动机器人视觉系统防抖装置由防抖机构、传感器系统和防抖控制系统三部分构成；所述防抖机构由左右移动单元、上下移动单元、俯仰单元、偏摆单元和基座22构成，上下移动单元、左右移动单元、俯仰单元和偏摆单元均设置在基座22上；所述传感器系统包括倾角传感器26、第一振动传感器24和第二振动传感器25，倾角传感器26、第一振动传感器24和第二振动传感器25均安装在基座22上，倾角传感器26分别测量基座22相对X轴方向和Y轴方向的转角；第一振动传感器24、第二振动传感器25分别测量基座22沿Z轴方向和Y轴方向的移动；所述防抖控制系统主要包括控制系统硬件和控制系统软件，用来实现视觉系统的防抖；

上下移动单元由上下驱动电机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同时做俯仰运动(俯仰驱动电机14通过第一俯仰绳轮15带动第二俯仰绳轮16及与第二俯仰绳轮16固接的支架17与两个视觉传感器27一起实现俯仰运动)；

偏摆单元由两个偏摆驱动电机9和两套偏摆单元传动机构构成；每套偏摆单元传动机构包括每一偏摆绳轮10、偏摆换向轮11、第一偏摆同步带轮12和第二偏摆同步带轮13；两个偏摆驱动电机9、两套偏摆单元传动机构左右对称设置，每个偏摆驱动电机9通过相应的偏摆单元传动机构实现每个视觉传感器27相对基座22偏摆运动；偏摆单元除具有实现装置的防抖功能外还实现了机器人的双目视觉；

倾角传感器26、第一振动传感器24和第二振动传感器25均安装在基座22上，倾角传感器26分别测量基座22相对Z轴方向和Y轴方向的转角；第一振动传感器24、第二振动传感器25分别测量基座22沿Z轴方向和Y轴方向的移动；

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

倾角传感器26用来测量基座22分别相对X轴方向和Y轴方向的转角

和

(X轴、Y轴所在坐标系的坐标原点位于基座的几何中心处)；该传感器为长沙平川公司提供的SCA100T-D01型双轴倾角传感器，测量范围为0.5g(±30度)，单极5V供电，比例电压输出，高分辨率，数字SPI或模拟输出，内置温度传感器，长期稳定性非常好，高分辨率，低噪声，工作温度范围宽。第一振动传感器24、第二振动传感器25分别用来测量基座22沿Z轴方向和Y轴方向的移动h₁、h₂；该传感器为长沙平川公司提供的N1000060(SCA60C)振动传感器，该传感器为直流响应，供电电压+5V，电流损耗低(典型植2mA)，与供电电压成比例输出，负载驱动能力强。本发明所述的被控对象——两个视觉传感器(CCD)27为VS-902型微型高清晰工业CCD摄像机，尺寸为38(W)×38(H)×50(D)，重量约250g，结构紧凑，外型小巧、功耗低。防抖控制系统23根据第一振动传感器24、第二振动传感器25、倾角传感器26分别测出来的抖动参数h₁、h₂、

进行抖动补偿计算，控制并驱动防抖机构上的两个视觉CCD传感器(两个视觉传感器27)相对地面保持平稳，一次性输出较清晰图像。本发明的防抖补偿控制方法通过对装置的动力学分析进行动态控制，采用前馈及PD反馈的控制方法实现装置的防抖目的。

本实施方式所述的移动机器人视觉系统防抖装置具有5个自由度：分别为CCD 27的两个独立的摆动、共同的俯仰自由度、整个装置的上下移动和左右移动。上下驱动电机1、左右驱动电机18、俯仰驱动电机14和一个偏摆驱动电机9控制一个视觉传感器27的位姿；上下驱动电机1、左右驱动电机18、俯仰驱动电机14和另一个偏摆驱动电机9控制另一个视觉传感器27的位姿。其中摆动和俯仰的轴线相交于一点类似人眼的转动，目的是为了获得机器人更大的视野范围。视觉系统防抖装置中上下移动单元至于左右移动单元之上，目的是为减小上下驱动电机1的负载。视觉系统防抖装置的宽为138mm，高为190mm，深为112mm，总质量为1.42kg。

具体实施方式二、结合图3、图4、图5、图6说明本实施方式，本实施方式所述的移动机器人视觉系统防抖装置的防抖补偿控制方法为：

步骤一、由第一振动传感器24、第二振动传感器25和倾角传感器26测出来的抖动参数h₁、h₂、

及相应的差分量确定测得抖动量变化的速度和加速度量；

步骤三、计算补偿量：建立各运动关节的D-H坐标系描述相邻杆件间平移a₁、a₂和转动θ₃、θ₄、θ₅的关系；通过D-H参数和其次变换矩阵法推导出各坐标系相对0坐标系的齐次变换矩阵，任意构形[a₁，a₂，θ₃，θ₄，θ₅}下两个CCD 27相对于装置基座22的位姿矩阵分别为⁰T_CCD4＝⁰A₁ ¹A₂ ²A₃ ³A₄、⁰T_CCD5＝⁰A₁ ¹A₂ ²A₃ ³A₅；当前构形

下两个CCD 27相对于装置基座22的位姿矩阵分别为

{\hat{T}}_{CCD 4}^{0} = {\hat{A}}_{1}^{0} {\hat{A}}_{2}^{1} {\hat{A}}_{3}^{2} {\hat{A}}_{4}^{3},

{\hat{T}}_{CCD 5}^{0} = {\hat{A}}_{1}^{0} {\hat{A}}_{2}^{1} {\hat{A}}_{3}^{2} {\hat{A}}_{5}^{3};

由第一振动传感器24、第二振动传感器25、倾角传感器26测出来的抖动参数h₁、h₂、

(进行差分后可得

)；确定CCD 27相对于装置基座22抖动的位姿矩阵为

则有如下关系式：

由上述两个式子在检测得到抖动参数h₁、h₂、

的情况下可推算出防抖机构在当前构形

(a_{1} - {\hat{a}}_{1}, a_{2} - {\hat{a}}_{2}, θ_{3} - {\hat{θ}}_{3}, θ_{4} - {\hat{θ}}_{4}),

(a_{1} - {\hat{a}}_{1}, a_{2} - {\hat{a}}_{2}, θ_{3} - {\hat{θ}}_{3}, θ_{5} - {\hat{θ}}_{5}),

即两个CCD27在理论上的抖动补偿量矢量分别为

Δq = (Δ q_{1}, Δ q_{2}, Δ q_{3}, Δ q_{4}) = (a_{1} - {\hat{a}}_{1}, a_{2} - {\hat{a}}_{2}, θ_{3} - {\hat{θ}}_{3}, θ_{4} - {\hat{θ}}_{4}),

Δq = (Δ q_{1}, Δ q_{2}, Δ q_{3}, Δ q_{4}) = (a_{1} - {\hat{a}}_{1}, a_{2} - {\hat{a}}_{2}, θ_{3} - {\hat{θ}}_{3}, θ_{5} - {\hat{θ}}_{5}),

进行微分后可得

则防抖装置各关节位置矢量为：

步骤五、基于PD反馈和逆动力学计算的抖动补偿控制：以

及该位置下速度

\overset{\cdot}{q} = \overset{\cdot}{q} ({\overset{\cdot}{q}}_{d 1}, {\overset{\cdot}{q}}_{d 2}, {\overset{\cdot}{q}}_{d 3}, {\overset{\cdot}{q}}_{d 4}),

加速度

\overset{\cdot \cdot}{q} = \overset{\cdot \cdot}{q} ({\overset{\cdot \cdot}{q}}_{d 1}, {\overset{\cdot \cdot}{q}}_{d 2}, {\overset{\cdot \cdot}{q}}_{d 3}, {\overset{\cdot \cdot}{q}}_{d 4})

图4为补偿量在第i个采样周期T内的4-3-4关节插值轨迹规划法得到的关节补偿量Δq_i与时间的关系曲线。求得关节补偿量Δq后，为了保证在采样时间T内能完成补偿过程，即关节补偿量Δq从0到Δq，而且在T时刻时关节角的速度和减速度都为0，必须对补偿的过程进行轨迹规划得到Δq(t)。轨迹规划出来的Δq(t)经过逆动力学计算得到要达到该关节角所应提供的力矩τ_i，直流电机将力矩τ_i传递给防抖装置，通过关节的转动和移动来进行抖动补偿。

图5为视觉系统防抖装置的动力学简化模型(该图含有各杆件质心、质量参数，是动力学模型必须表示的)。通过视觉系统防抖装置的动力学简化模型，分析影响系统性能指标的各种因素，设计出合理的系统控制方案。图5中的结构模型各参数的含义如表1所示。同时取(装置)基座22所在的平面为零势能面。

表1模型中各参数含义

引入拉格朗日方程法建立机器人的动力学方程：

τ_{i} = \frac{d}{dt} \frac{&PartialD; L}{&PartialD; {\overset{\cdot}{q}}_{i}} - \frac{&PartialD; L}{&PartialD; q_{i}}, i = 1,2,3,4,5;

经测量和参数识别实验确定质心位置、质量、转动惯量以及粘性摩擦和动摩擦项系数矩阵等，得到较为精确的动力学方程式矢量表示形式：

τ (q, \overset{\cdot}{q}, \overset{\cdot \cdot}{q}) = \tilde{M} (q) \overset{\cdot \cdot}{q} + \tilde{C} (q, \overset{\cdot}{q}) + \tilde{B} \overset{\cdot}{q} + \tilde{D} (\overset{\cdot}{q}) + \tilde{g} (q)

(其中，

)用于进行基于逆动力学计算的前馈，以减小抖动补偿控制的误差。其中，变量上方的“～”号表示经参数识别后的量，如

分别表示经参数识别实验和参数识别算法计算后得到的惯性力项的系数矩阵、离心力与科氏力项、粘性摩擦项系数矩阵、动摩擦项、重力项。

图6为基于PD反馈和逆动力学前馈的控制器框图。控制输入为防抖装置各关节期望得到补偿的关节位置

及该位置下速度、加速度，这里下标“d”表示期望得到补偿。此外，变量上方的“^”号表示进行抖动补偿之前的关节当前位置量，如

表示关节4当前位置量。

具体实施方式三、下面结合图7说明本实施方式，本发明所述视觉系统防抖装置可应用在仿人机器人30上。视觉系统防抖装置可作为仿人机器人的头部，通过装置基座22与仿人机器人30上的颈部31上的接口固连。视觉系统防抖装置中的防抖控制系统23作为防抖控制的底层控制系统，通过控制线、数据线与仿人机器人30胸部内的上位控制机相连。

具体实施方式四：结合图8说明本实施方式，本发明所述视觉系统防抖装置可作为仿人机器人的头部，通过装置基座22与轮式移动机器人32上的颈部33上的接口固连。视觉系统防抖装置中的防抖控制系统23作为防抖控制的底层控制系统，通过控制线、数据线与轮式移动机器人32胸部内的上位控制机相连。

本发明所述视觉系统防抖装置在移动机器人中应用不仅限制在以上两种移动机器人上，还包括其他类型的移动机器人如履带式移动机器人等。

具体实施方式五：结合图9说明本实施方式。采用PD控制+逆动力学计算法进行抖动补偿控制仿真。用

表示两个视觉传感器27相对于地面的位姿参数。图9a为无抖动时即期望的CCD相对于地面的位姿曲线；在图9b所示抖动频率f＝20时随机抖动参数h₁、h₂曲线(纵轴表示抖动位移量，单位为m，抖动位移幅度为±1mm；横轴表示时间，单位为s)、抖动偏摆幅度a₁、a₂皆为±5°、传感器的响应频率为20Hz(即采样时间为0.05s)的情况下，利用Matlab软件进行有抖动不补偿控制和抖动补偿控制，控制仿真结果如图9c和9d所示。9c为有抖动不补偿时CCD相对于地面的位姿曲线；图9d为进行抖动补偿时CCD相对于地面的位姿曲线。由图9c和图9a对比可以明显看出：未经补偿的图9c所示的位姿曲线表明抖动给视觉传感器27带来了较大的位姿误差，其结果将导致图像模糊和较大的视觉定位误差；而由图9d和图9a对比可以明显看出：采用抖动补偿后的27各项位姿参数曲线图9d能够较为精确地接近于图9a所示的期望位姿参数曲线。

Claims

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₂、