[发明专利]一种挂装车外挂物位姿调整方法

权利要求书

1.一种挂装车外挂物位姿调整方法，其特征在于，所述挂装车外挂物位姿调整方法包括以下步骤：

S1：利用智能识别技术和惯性数据计算得到目标点位姿坐标，且还包括以下子步骤：

S11：建立位置坐标系{A}，设空间中任一点P的位置可用3×1的列矢量^AP表示为：

式中，上角标A代表位置坐标系{A}，^AP为位置矢量；

S12：建立姿态坐标系{B}，设坐标系{B}的三个相互垂直的单位矢量为x_B，y_B，z_B，对于空间任意矢量V，其方向由该矢量与三个坐标轴夹角的方向余弦表示为：

即坐标系{B}的三个单位矢量x_B，y_B，z_B相对于坐标系{A}的方向余弦组成3×1矩阵^AR_B表示外挂物姿态：

式中：n_x——坐标系{B}的X轴对坐标系{A}X轴的余弦；

n_y——坐标系{B}的X轴对坐标系{A}Y轴的余弦；

n_Z——坐标系{B}的X轴对坐标系{A}Z轴的余弦；

o_x——坐标系{B}的Y轴对坐标系{A}X轴的余弦；

o_y——坐标系{B}的Y轴对坐标系{A}Y轴的余弦；

o_Z——坐标系{B}的Y轴对坐标系{A}Z轴的余弦；

a_x——坐标系{B}的Z轴对坐标系{A}X轴的余弦；

a_y——坐标系{B}的Z轴对坐标系{A}Y轴的余弦；

a_Z——坐标系{B}的Z轴对坐标系{A}Z轴的余弦；

该矩阵即表示外挂物相对于坐标系{A}的姿态，^AR_B被称为B的旋转矩阵；

S13：将外挂物与坐标系{B}固接，将外挂物的几何中心设置为{B}的坐标原点；

S14：用坐标系{B}表示外挂物的位姿为：

{B}＝{^AR_B,^AP_B0} (4)；

S2：利用挂装装置调姿平台上的运动传感器，得到运动关节位姿坐标；

S3：利用DH算法对运动方程逆解求取运动关节转角和位移

S4：判断目标位姿是否可达，若目标位姿可达，则对运动关节进行运动规划，得到每个关节运动的先后顺序、运动时间，并输出到控制器；若目标位姿不可达，则发出警示标志，对目标位姿进行修正；

S5：通过控制器控制调姿平台运动以达到目标位姿。

2.如权利要求1所述的一种挂装车外挂物位姿调整方法，其特征在于，所述挂装车外挂物位姿调整方法还包括：

S6：进行多次微调，以达到要求的调姿精度。

3.如权利要求1所述的一种挂装车外挂物位姿调整方法，其特征在于，所述步骤S2还包括以下子步骤：

S21：制定关节连杆命名规则：指定第一个关节为关节n，第二个关节为关节n+1，其余关节以此类推；指定第一个连杆为连杆n，第二个连杆为连杆n+1，其余连杆以此类推；

S22：确定Z轴方向：若关节是旋转的，则Z轴方向为按右手规则旋转的方向；若关节是滑动的，则Z轴方向为沿直线运动的方向，关节n处Z轴下标为n-1；

S23：确定X轴方向：若两关节的Z轴既不平行也不相交，则取两关节Z轴公垂线的方向作为X轴方向，命名规则同Z轴；若两关节的Z轴平行，则挑选两Z轴之间与前一关节的公垂线共线的一条公垂线为X轴方向；若两关节的Z轴相交，则取两关节Z轴的叉积方向为X轴方向；

S24：确定Y轴方向：取X轴、Y轴差积方向，按右手规则确定Y轴的方向；

S25：根据X、Y、Z轴方向建立坐标系。

4.如权利要求1所述的一种挂装车外挂物位姿调整方法，其特征在于，所述步骤S3还包括以下子步骤：

S31：将已知姿态矩阵的左右两侧依次乘以各机构齐次变换的逆矩阵；

S32：选择矩阵中具有常数项的元素，用左右两边同时相乘的方法得到转角和位移。