cos0i;
[0082] Sjj------表示 sin ( θ，Θ ;
[0083] ciS-------表示 cos ( Θ θ )
表示坐标系1和坐标系〇的齐次变换矩阵
和姿态。
[0091] η-手的法向矢量
[0092] s-手的滑动矢量
[0093] a一手的接近矢量
[0094] p-手的位置矢量（6)
[0095] 坐标系Oji是正整数i = 0, 1，......，6)则是在操作臂上建立的D-Η坐标系； a2, d4e R分别表示机械臂对应连杆的长度。可以将操作臂末端坐标系〇 6在基座坐标系〇。 下的位姿写成如下表达式：°T6= A 。求解运动方程时，从°T6开始求解关节位置。 使°Τ6的符号表达式的各元素等于°Τ6的一般形式，并据此确定Θ i。一旦求得之后，可 由A/左乘°T6的一般形式，得
[0096] Α11°Τ6= X (7)
[0097] 此式可用来求解其他各关节变量。不断地用Α的逆矩阵左乘（7)，可得下列另四个 矩阵方程式：
[0098] A21A11 °T6= 2T6 (8)
[0099] A3lk21A11 °T6= 3T6(9)
[0100] a4jA3jA2 % 1 °T6= 4T6(10)
[0101] a51 0T6= 5T6(11)
[0102] 上式各方程的左式为°T6和前（i-1)个关节变量的函数，可用这些方程来确定各关 节的位置：
[0106] 注意：式中，正、负号对应03的两种可能解。
[0110] 注意：当s5= 0时，机械臂处于奇异形位。此时，关节轴4和6重合，只能解出Θ 4 与96的和或差。奇异形位可以由式（15)中atan2的两个变量是否都接近零来判断。
[0111] Θ 5= a tan2 (_a x (CiCnq+SiSj _ay (SiCmq-CiSj +azs23c4, -axCiSja-aySnSfazC；^)
[0112] (-3. 9268 ^ Θ ^ 〇. 7853) (16)
[0113] k! = _n x (CiCj^-s^) _ny (SiCj^+c^) +nzs23s4
[0114] k2= n x ((CiCnq+SiSj C5-CP23S5) +ny ((SiCnCfCiSj C5-S023S5) -nz (s23c4c5+c23s5)
[0115] Θ 6= a tan2 (k 1; k2) (-3. 1415 ^ 0^3. 1415) (17)
[0116] 上述虽然结合附图对本发明的【具体实施方式】进行了描述，但并非对本发明保护范 围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不 需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。
【主权项】
1. 大持重液压小吊臂的控制系统，其特征是，包括：遥控器通过光纤收发模块与工控 机通信，工控机连接信号处理模块，信号处理模块输出的模拟量通过伺服放大器与多个液 压伺服阀连接，信号处理模块输出的数字量传送至多个开关阀，遥控器的指令通过光纤收 发模块下发给工控机，工控机通过信号处理模块下发控制指令、开关驱动指令给小吊臂。2. 如权利要求1所述的大持重液压小吊臂的控制系统，其特征是，所述液压伺服阀分 别与对应的小吊臂机械臂的第一腰部回转轴、大臂俯仰轴、小臂俯仰轴、第二腰部回转轴及 手爪开合轴相连。第一腰部回转轴、大臂俯仰轴、小臂俯仰轴、第二腰部回转轴及手爪开合 轴分别与对应的模拟电位器相连，模拟电位器均通过传感器集束系统连接信号处理模块。3. 如权利要求1所述的大持重液压小吊臂的控制系统，其特征是，所述遥控器包括：模 拟量采集模块、数字量采集模块分别通过磁藕隔离模块I、磁藕隔离模块II与微处理器连 接，微处理器与磁藕隔离模块III、串口驱动电路、光纤发送模块依次连接，复位电路、JTAG 电路、晶振电路分别与微处理器连接，模拟量采集模块采集主手电位器的数据，数字量采集 模块采集带锁按键、自复位按键的数据，并经过微处理器处理后，经过光纤收发模块，发送 给工控机。4. 如权利要求1所述的大持重液压小吊臂的控制系统，其特征是，所述遥控器带有5自 由度模拟主手，用于控制5自由度小吊臂每个关节的运动速度、位置；遥控器上设置11个按 键，分别用于控制液压开关、主/从坐标系切换键、手爪锁止、小吊臂暂停、一键还原、X轴正 方向世界坐标系运动、X轴负方向世界坐标系运动、Y轴正方向世界坐标系运动、Y轴负方向 世界坐标系运动、Z轴正方向世界坐标系运动、Z轴负方向世界坐标系运动；遥控器上还设 置7组状态指示灯，即液压开关、手爪锁止、小吊臂暂停、一键还原、单轴运动、世界坐标系 运动状态指示、安全报警指示。5. 如权利要求1所述的大持重液压小吊臂的控制系统，其特征是，所述模拟量采集模 块采集5自由度模拟主手信号，电压范围O~10V，数字量采集模块采集11个按键的开关信 号，高电平5V、低电平0V，数字量输出模块输出开关信号控制7组状态指示灯。6. 如权利要求1所述的大持重液压小吊臂的控制系统，其特征是，所述磁藕隔离模块 I、磁藕隔离模块II与磁藕隔离模块III均为四通道数字隔离器，两端工作电压2. 7V~ 5. 5V，支持低电压工作并能实现电平转换； 所述信号处理模块输出5路模拟量信号，电流范围O~20ma ;输出1路开关量信号。7. 如权利要求1所述的大持重液压小吊臂的控制系统，其特征是，所述小吊臂是5自由 度液压机械臂；角度传感器设于小吊臂上，角度传感器测量机械臂每个轴的角度大小，机械 臂机械结构包括基座，基座上安装有大臂，大臂可以实现腰部回转、大臂俯仰，上端为肩关 节，小臂安装在肩关节上，小臂可以实现腕部回转，小臂的前端与手爪连接。8. 如权利要求1所述的大持重液压小吊臂的控制系统，其特征是，大持重液压小吊臂 主手控制系统控制时分为主端和从端两部分，主端发送命令给从端，从端反馈信息给主端， 操作主手运动，小吊臂控制器实时采集小吊臂电位器的位置信息并将位置信息发送给主手 控制器，机械臂控制器通过对主手的位置跟踪来完成机械臂的运动，同时主手控制器再将 该位置信息发送给手持终端，供显示。9. 大持重液压小吊臂的控制方法，其特征是，包括如下步骤： 1)对每个杆件在关节轴处建立一个笛卡儿坐标系（Xl，yi， Zl)，i是1到5之间的所有正 整数，5为自由度数目，再加上基座坐标系（xQ，yQ，z。）； 2) 为每个关节处的杆件坐标系建立4X4奇次变换矩阵，表示与前一个杆件即当前杆 件的前一个杆件的坐标系的关系； 3) 采用"边算边走"的定时插补算法，计算插补点的位置和姿态；"边算边走"是指将 各插补点进行逆运动学变换后得到的关节位置不用存储，而直接再按这些关节位置开始运 动； 4) 计算每个轴的运动学反解，运动学反解是指已知末端位置和姿态求每个关节的角 度，得出插补周期内的每个轴的运动角度。10.如权利要求9所述的大持重液压小吊臂的控制方法，其特征是，所述步骤中1)中 确定和建立每个坐标系采用下面三条规则：每个关节i的运动都绕着Z1轴运动,轴垂直 Z1 i轴并指向离开z i i轴的方向；y 4由按右手坐标系得要求建立。
【专利摘要】本发明公开了大持重液压小吊臂的控制系统及其控制方法，遥控器通过光纤收发模块与工控机通信，工控机连接信号处理模块，信号处理模块输出的模拟量通过伺服放大器与多个液压伺服阀连接，角度传感器设于小吊臂上，且通过传感器集束系统连接信号处理模块，遥控器的指令通过光纤收发模块下发给工控机，工控机通过信号处理模块下发控制指令、开关驱动指令给小吊臂。本发明自行设计的大持重液压小吊臂控制系统经实验验证，代替人的手臂完成10kV配电带电更换横担、变压器重载任务。
【IPC分类】B25J9/16, B25J9/20
【公开号】CN105234942
【申请号】CN201510735630
【发明人】赵玉良, 戚晖, 李玉华, 阮鹏程, 王海磊, 慕世友, 傅孟潮, 李勇, 吴观斌, 许乃媛, 李建祥, 赵金龙, 刘庭
【申请人】国网山东省电力公司电力科学研究院, 山东鲁能智能技术有限公司, 国家电网公司
【公开日】2016年1月13日
【申请日】2015年11月2日