一种双臂巡检机器人质心自动调整控制系统和方法与流程

本发明涉及巡检机器人质心控制领域，具体地说是一种双臂巡检机器人质心自动调整控制系统和方法。

背景技术：

轮臂复合结构的输电线巡检机器人是当前巡检机器人中的常见类型，这种机器人通过轮臂复合机构实现机器人在架空输电线上行走及跨越线路障碍。机器人在跨越障碍时需要在单臂挂线状态下调节各关节运动使脱线手臂跨越障碍物。机器人跨越障碍时的姿态变化使得机器人的质量分布发生变化，从而造成倾斜，导致机器人承受附加倾覆力矩而处于不稳定状态，尽管挂线手臂通过夹线机构能够提供一定的夹紧力，但是过大的夹紧力容易损坏挂线处的输电线路。由于机械结构限制，机器人倾斜还容易使脱线手臂与线路发生干涉。因此保持机器人越障时处于动态稳定状态是实现机器人越障的关键。

机器人的动态稳定状态是指在机器人单臂挂线运动时通过实时调整机器人质心位置而保持机器人处于水平姿态的状态。邬大为等人通过模型计算，规划配重块的位置以调整机器人质心实现机器人的稳定运动，这种方法对建模精度要求很高[邬大为，阮毅，任志斌.基于产生式系统和轨迹优化的巡线机器人控制[J].计算机工程与设计，2008，29(11)：2868-2870.]；朱兴龙等人提出一种通过控制配重块位置调整质心的方法，利用静力矩平衡方程初步估计配重块位置，当配重块到达估计位置后再利用倾角传感器的输出调整配重块位置使机器人稳定在水平姿态，这种方法可以保证机器人最终在稳定状态，但是无法保证运动过程中机器人始终处于稳定状态，即无法保证机器人质心的动态稳定[朱兴龙，王洪光，房立金，等.自主越障巡检机器人质心调节控制[J].机器人，2006，28(4)：385-388.]。

技术实现要素：

针对现有技术的不足，本发明提供一种双臂巡检机器人质心自动调整控制系统和方法，能在建模误差和干扰的情况下，使机器人双臂交错运动时保持稳定状态。

本发明为实现上述目的所采用的技术方案是：

一种双臂巡检机器人质心自动调整控制系统，倾角传感器设置于机器人的控制箱内，且通过A/D转换电路连接处理器，检测机器人的倾斜角度并输出对应的模拟电压信号，通过A/D转换电路转换为数字信号发送给处理器；

位移传感器设置于每个关节的运动电机上，且连接处理器，采集各个关节相对于各自零点的位移，发送到处理器；

处理器通过RS232通信转换器连接关节电机驱动器，发送控制命令到关节电机驱动器，驱动电机运动。

所述位移传感器包括前臂5关节位移传感器、后臂5关节位移传感器和6关节位移传感器。

所述关节电机驱动器包括前臂5关节电机驱动器、后臂5关节电机驱动器和6关节电机驱动器。

一种双臂巡检机器人质心自动调整控制方法，建立机器人单臂挂线的动力学模型；根据建立的动力学模型设计控制器，完成质心调整。

所述建立机器人单臂挂线的动力学模型包括以下过程：

在后臂挂线运动中，在后臂悬挂点中心建立基坐标X₀Y₀Z₀，假设该基坐标固定；

机器人有9个运动关节，设m_i(i＝0,1，…,9)为各连杆的质量，s_i为各连杆的质心在本连杆坐标系中的位置，l_i为机器人在初始位置时各质心之间的连接长度；θ为机器人水平倾斜角度，D为导轨总长；

在此基础上，建立机器人各连杆的D-H坐标系。

机器人在后臂挂线状态下运动时，后臂腕关节电机处于自由状态；施加在后臂腕关节处的力矩为：

τ₁＝¹Z₁¹n₁ (1)式中：τ₁由后臂俯仰关节被动机构提供的阻尼决定；阻尼所提供的力矩与后臂俯仰角度θ-α成反比，其中α为输电导线与水平面的夹角，k为阻尼系数。则：

τ₁＝-k(θ-α) (2)由式(1)和式(2)得机器人绕关节1的z轴的力矩平衡方程为：

-k(θ-α)＝¹Z₁¹n₁ (3)

机器人运动过程中，倾斜角度θ一般不会过大，设-10°≤θ≤10°，故可近似cosθ≈1，sinθ≈θ，将式(3)整理得：

式中：

J＝∑(m_i|⁰s_i|²+I_i)

g_u＝g[-m₅ m₆ m₃ m₈ m₁ m₉]·[l₁ l₃ l₄ l₅ l₆ l₇]^T+g[m₈₉ m₁][d₈ d₂]^T

f₁＝[m₅l₁₇ m₆₇₈₉l₁₇ m₄₅₆₇₈₉l₁₇]

f₂＝[m₅ m₆₇₈₉ m₄₅₆₇₈₉]

f₃＝m₃gl₂-Dm₄₆₇₈₉g-m₉grsθ₇+kα

u＝[d₅ d₄ d₆]^T

其中，θ_i和d_i表示关节i的关节变量，I_i表示连杆i的惯性矩阵；

机器人后臂挂线，调整两臂间距时，一般只需调节关节4、关节5和关节6位置，关节1被动适应，其他关节保持不变，即

所述控制器的设计包括以下步骤：在质心调整过程中，如控制箱体(d₅)可调， 由控制器K_c1调节，通过实时调节d₅保持机器人平稳，并通过协调d₄、d₆使两臂间距Δ按规划运动；当控制箱体d₅不可调时切换至控制器K_c2，通过协调两臂运动(d₄,d₆)保持机器人平稳，且使两臂间距Δ按规划运动。控制器切换由Q触发，P为规划器；

(1)规划器P的设计

机器人静止时，满足静力矩平衡方程：

-k(θ-α)-∑m_ig(⁰s_i⁰Y₀)＝0 (6)

以巡检机器人后臂悬挂在输电线上，保持机器人在稳定状态调整两臂间距由Δ₀至Δ为研究对象。机器人通过双臂导轨和控制箱移动关节相互配合实现由初始位置保持θ＝0调整两臂间距至Δ。关节变量的期望值为(θ₁,d₂,θ₃,d₄,d₅,d₆,θ₇,d₈,θ₉)，调整过程中保持：关节变量的期望值满足式式(6)，整理得：

式中：b⁰(θ⁰)和b⁰(θ)是与机器人参数、关节变量初始值以及θ、θ⁰和α有关的参数；

由于机器人3个水平移动关节最大速度相同，则机器人调整时间最短即3个水平移动关节的最大位移最小；3个水平移动关节的期望值(d₅,d₆,d₄)应满足：

采取遍历变量的方法解式式(8)得满足调整时间最短的机器人期望关节变量；

(2)d₅可调，设计控制器K_c1

令d＝d_r-Δd，则e_Δ＝Δ_r-Δ＝-Δd₆-Δd₄。代入式(4)得：

其中：h＝f₂u_r+f₁u+f₃+m₆₇₈₉ge_Δ-m₄₅gΔd₄，令e＝θ_r-θ，则

将式(9)转化为：

式中：u＝[Δd₅ Δd₆]^T，C＝[1 0]，D₁＝D₂＝0，ω＝h-J_tθ_r+g_uθ_r；

设计H_∞最优状态反馈控制器u＝W(X)^-1x，即又Δd₄＝-e_Δ-Δd₆，得：

(3)d₅不可调，设计控制器K_c2

将e_Δ＝-Δd₆-Δd₄。代入式(4)得：

式中：h＝f₂u_r+f₁u+f₃+m₄₅₆₇₈₉ge_Δ+m₅gΔd₅；

设计H_∞最优状态反馈控制器u＝W(X)^-1x，即又得：

本发明具有以下有益效果及优点：

本发明能够在存在建模误差和干扰的情况下，使机器人在双臂交错运动时保持稳定状态。

附图说明

图1巡检机器人结构。

图2机器人初始位置及坐标系。

图3后臂悬挂的几何关系。

图4质心调整控制器结构。

图5仿真中机器人倾角的开环和闭环响应。

图6仿真中加入干扰后倾角的开环和闭环响应。

图7质心调节实验。

图8试验中机器人倾角的开环和闭环响应。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

巡检机器人由行走越障机构、质心调整机构和控制箱体组成，如图1所示。机器人行走越障机构由两个轮-爪-臂(复合手臂)组成，每个手臂分别有行走关节(图1(1))、腕关节(图1(2))、伸缩关节(图1(3))和旋转关节(图1(4))，手臂末端安装夹爪，可以抓握导线。机器人质心调节机构由双臂导轨和控制箱导轨组成，分别由3个移动关节驱动(图1(5)和(图1(6))，可以实现两臂和控制箱沿导轨水平运动；控制箱体装有机器人控制系统及电源等，同时具有机器人质心配重块的 作用。

(1)建立机器人单臂挂线的动力学模型

以后臂挂线运动为例，忽略输电线振动和变形对机器人的影响，在后臂悬挂点中心建立基坐标X₀Y₀Z₀，假设该基坐标固定。机器人有9个运动关节，设m_i(i＝0,1，…,9)为各连杆的质量，s_i为各连杆的质心在本连杆坐标系中的位置，l_i为机器人在初始位置时各质心之间的连接长度，如图2(a)所示。θ为机器人水平倾斜角度，D为导轨总长。在此基础上，建立机器人各连杆的D-H坐标系，如图2(b)所示。机器人在后臂挂线状态下运动时，后臂腕关节(关节1)电机处于自由状态。施加在关节1处的力矩为：τ₁＝¹Z₁¹n₁式中：τ₁由后臂俯仰关节被动机构提供的阻尼决定。阻尼所提供的力矩与后臂俯仰角度θ-α成反比，其中α为输电导线与水平面的夹角，k为阻尼系数，如图3所示。则：τ₁＝-k(θ-α)。机器人运动过程中，倾斜角度θ一般不会过大，设-10°≤θ≤10°，故可近似cosθ≈1，sinθ≈θ，将机器人绕关节1的z轴的力矩平衡方程整理为：仿真实验中使用的巡检机器人模型参数为：D＝0.78,J≈1,g_u＝-19.168,f₁＝[1.02 0.6-2.04],f₂＝[17 10.01 34.01],f₃＝-24。由于建模误差的存在，实际机器人参数与模型存在偏差，假设实际机器人参数为：f₁＝[1.2 0.66 -2.1],f₂＝[20 11 35]，其他参数相同。

(2)状态反馈控制器的设计

设计质心调整控制器的结构如图4所示。

在质心调整过程中，如控制箱体(d₅)可调，由控制器K_c1调节，通过实时调节d₅保持机器人平稳，并通过协调d₄、d₆使两臂间距Δ按规划运动；当控制箱体d₅不可调时切换至控制器K_c2，通过协调两臂运动(d₄,d₆)保持机器人平稳，且使两臂间距Δ按规划运动。控制器切换由Q触发，P为规划器。

(2.1)设计规划器P

以巡检机器人后臂悬挂在输电线上，保持机器人在稳定状态调整两臂间距由Δ₀至Δ为研究对象。机器人通过双臂导轨和控制箱移动关节相互配合实现由初始位置保持θ＝0调整两臂间距至Δ。关节变量的期望值为(θ₁,d₂,θ₃,d₄,d₅,d₆,θ₇,d₈,θ₉)，调整过程中保持

由于机器人3个水平移动关节最大速度相同，则机器人调整时间最短即3个水平移动关节的最大位移最小。3个水平移动关节的期望值(d₅,d₆,d₄)应满足：

仿真实验中机器人的初始条件为，两臂间距Δ°＝0.1，初始偏角θ⁰＝0.098。调整后机器人状态为Δ＝0.3，偏角θ＝0。解规划器P，即机器人的期望位置：两臂间距Δ＝0.3，偏角θ＝0。

(2.2)d₅可调，设计控制器K_c1

关节1的z轴的力矩平衡方程整理为：将其转化为：

式中：u＝[Δd₅ Δd₆]^T，C＝[1 0]，D₁＝D₂＝0，ω＝h-J_tθ_r+g_uθ_r。

设计H_∞最优状态反馈控制器u＝W(X)^-1x得：

仿真实验中：

(2.3)d₅不可调，设计控制器K_c2

关节1的z轴的力矩平衡方程整理为：

设计H_∞最优状态反馈控制器u＝W(X)^-1x得：

仿真实验中：

图5是存在建模误差情况下，机器人的开环和闭环响应曲线。结果表明所设计的控制器可以保证机器人系统的稳定性，能在建模误差存在的情况下，将机器人倾角偏差调节到能够接受的范围内，从而保证了机器人在调整质心过程中保持稳定状态。图6是在运动过程中，机器人同时受到风载外力矩干扰和关 节速度响应干扰时，机器人的响应曲线对比，结果表明控制器能抑制干扰对倾角的影响，使机器人平稳并保持两臂间距按规划值变化。其中虚线为不加控制作用时的开环响应曲线，实线为有控制作用时的闭环响应曲线。

在实验室模拟的输电线路上进行巡检机器人质心调节实验，如图7所示。实验内容为:后臂行走轮落在导线上，前臂抬起，将两臂间距由300mm调整至-300mm，机器人初始的倾角为-5.2°，机器人倾角的开环和闭环响应曲线如图8所示。实验表明在两臂间距调整过程中机器人两手臂和控制箱沿导轨协调运动，机器人倾角稳定在设定值0°。