[发明专利]基于倾角传感器的动态绳索摆角测量方法及系统

权利要求书

1.一种基于倾角传感器的动态绳索摆角测量方法，其特征在于，包括以下步骤：

S1，利用相机拍摄绳索在不同摆角时的若干个图像作为样本图像；

S2，求取各个样本图像中绳索运动端在样本图像中的像素坐标以及绳索运动端在实际空间中的世界坐标；

S3，根据各个样本图像中绳索运动端的像素坐标与世界坐标求取透视投影成像矩阵；

S4，根据透视投影成像矩阵完成相机的标定；

S5，利用步骤S4中完成标定的相机拍摄绳索的实时摆动图像，并计算实时摆动图像中绳索运动端的像素坐标；

S6，根据相机的参数与实时摆动图像中绳索运动端的像素坐标求取实时摆动图像中绳索运动端的世界坐标；

S7，利用倾角传感器测量相机的晃动量，并根据倾角传感器测得的相机的晃动量校正实时摆动图像中绳索运动端的世界坐标，获取去除晃动误差的绳索运动端的世界坐标；

S8，根据实时摆动图像中绳索运动端的世界坐标以及绳索固定端的世界坐标求取绳索的摆角；

其中，设定H_gij为相机抖动前后倾角传感器坐标系之间相对位置姿态的齐次变换矩阵，H_cij为相机抖动前后相机坐标系之间相对位置姿态的齐次变换矩阵，H_cg为倾角传感器坐标系和相机坐标系之间的相对位置姿态齐次矩阵，H_ci为抖动前相机与目标的相对位置姿态齐次矩阵，H_cj为抖动后相机与目标的相对位置姿态齐次矩阵，R_gij、R_cij和R_cg为旋转矩阵，T_gij、T_cij和T_cg为平移向量；

所述利用倾角传感器测量相机的晃动量，并根据倾角传感器测得的相机的晃动量校正实时摆动图像中绳索运动端的世界坐标，获取去除晃动误差的绳索运动端的世界坐标，包括以下步骤：

S71，基于罗德里格斯变换，利用以下公式7将旋转矩阵转换为旋转向量；

r_gij和r_cij为旋转向量，rodrigues()为罗德里格斯变换公式；

S72，利用以下公式8进行向量归一化；

S73，利用以下公式9，以修正的罗德里格斯参数表示姿态变化；

P_gij和P_cij表示旋转向量r_gij和r_cij修正的罗德里格斯参数；

S74，利用以下公式10计算初始旋转向量P′_cg；

skew(P_gij+P_cij)P′_cg＝P_cij-P_gij 公式10

skew为反对称运算；

S75，利用以下公式12计算旋转向量P_cg；

S76，利用以下公式13计算旋转矩阵R_cg；

I表示单位矩阵；

S77，利用以下公式14计算平移向量T_cg；

(R_gij-I)T_cg＝R_cgT_cij-T_gij 公式14

S78，将步骤S6中求取的实时摆动图像中绳索运动端的世界坐标与倾角传感器坐标系和相机坐标系之间的相对位置姿态齐次矩阵H_cg相乘，得到去除晃动误差的绳索运动端的世界坐标。

2.根据权利要求1所述的基于倾角传感器的动态绳索摆角测量方法，其特征在于，所述步骤S2中，利用全站仪求取各个样本图像中绳索运动端在实际空间中的世界坐标。