[发明专利]一种塔尺/棱镜杆竖向自稳控制装置及控制方法

摘要

塔尺/棱镜杆竖向自稳控制装置及方法，装置中三级动臂通过转接机构与夹具连接，二级动臂通过一个X轴转动关节与三级动臂连接、X轴伺服电机内置于该X轴转动关节，一级动臂通过Z轴转动关节与二级动臂连接、Z轴伺服电机内置于该Z轴转动关节，手持握杆通过另一个X轴转动关节与一级动臂连接，传感器安装在夹具上。方法包括S1：采样T_(i)时刻的姿态倾角θ_x(i)、θ_z(i)；S2：计算姿态偏差E_x(i)、E_z(i)；S3：进行PID计算；得出补偿角度信号O_x(i)、O_z(i)。S4：X轴伺服电机和Z轴伺服电机执行补偿动作。具有结构简单可靠、响应速度快、执行效率高、自动化程度高、可减轻人工扶尺的劳动强度的优点。

主权项

1.一种塔尺/棱镜杆竖向自稳控制装置，定义塔尺/棱镜杆垂直状态的轴向为Z轴，与Z轴垂直的水平面定义X轴和Y轴，其特征在于：包括手持握杆(1)、一级动臂(2)、二级动臂(3)、三级动臂(4)、两个X轴转动关节(5)、一个Z轴转动关节(6)、X轴伺服电机(7)、Z轴伺服电机(8)、转接机构(9)、夹具(10)和传感器(11)，所述夹具(10)夹持在塔尺/棱镜杆上，所述三级动臂(4)通过转接机构(9)与夹具(10)连接，所述二级动臂(3)通过一个X轴转动关节(5)与三级动臂(4)连接、所述X轴伺服电机(7)内置于该X轴转动关节(5)内，所述一级动臂(2)通过Z轴转动关节(6)与二级动臂(3)连接、Z轴伺服电机(8)内置于该Z轴转动关节(6)内，所述手持握杆(1)通过另一个X轴转动关节(5)与一级动臂(2)连接，所述传感器(11)安装在夹具(10)上，所述转接机构(9)包括固定台(91)、X向铰接件(92)、Y向铰接件(93)和Z向铰接件(94)，所述固定台(91)与夹具(10)固接，所述Z向铰接件(94)与固定台(91)活动套接并能绕Z向转动，所述Y向铰接件(93)与Z向铰接件(94)活动铰接并能绕Y向转动，所述X向铰接件(92)与Y向铰接件(93)活动铰接并能绕X向转动，所述X向铰接件(92)连接所述三级动臂(4)，固定台(91)包括U型台(911)和滑杆(912)，所述U型台(911)的背板与夹具(10)固接，所述滑杆(912)固装在U型台(911)的顶底板之间，所述Z向铰接件(94)滑套在滑杆(912)上。