[发明专利]一种基于车体位姿的作业部件末端位姿测量方法

权利要求书

1.一种基于车体位姿的作业部件末端位姿测量方法，其特征在于：包括如下步骤：

a、将姿态传感器安装于车体或与车体刚性连接的载体平台上，测量车体姿态；

b、将GNSS系统中的卫星天线安装于车体顶部，测量车体的位姿信息；或将激光传感安装于车体或载体平台上，测量基于基准作业面的相对位置信息；

c、建立车体坐标系Oxyz，以车体前进方向为y轴，x轴与y轴垂直指向车体方向的右侧，z轴垂直于x、y轴向上；

d、在各个执行机构安装传感器，检测执行机构的姿态变化以及位置变化，并建立作业部件坐标系Ox_ny_nz_n，n＝1,2...；

e、获取卫星天线或激光传感的位置信息，获取车体姿态信息，获取执行机构的姿态信息及位置变化量，建立基于所获取信息及执行机构几何关系求解作业部件末端中心的位置解算模型，并计算获得作业部件末端中心坐标；

f、测量作业部件的姿态信息并推算作业部件末端的姿态信息；

g、设计基准作业面，结合所求的作业部件末端位姿信息计算作业部件末端与基准作业面的高程差Δh，以及作业部件末端与基准作业面的角度差Δα；

步骤e中，根据卫星天线的位置信息、车体姿态信息和各执行机构的姿态信息及位置变化量，建立GNSS坐标解算模型，解算作业部件末端三维坐标，GNSS坐标解算模型如下：

其中，n＝1,2,...；r表示作业部件末端中心的三维坐标；p表示卫星天线的位置信息；ψ,θ分别为车体偏航角、横滚角和俯仰角；β₁,β₂,......,β_n为各执行机构上姿态传感器测量值；l₁,l₂,......,l_n为各执行机构上位移传感器测量值；

解算作业部件末端三维坐标步骤如下：

(1)测量车体及执行机构的角度与长度信息和卫星天线至车体质心位置的坐标增量，输入GNSS坐标解算模型；

(2)换算车体姿态角；

(3)计算车身姿态旋转矩阵；

(4)根据几何关系或机器人运动学及所测量的各执行机构姿态信息与位置变化量，解算基于车体坐标系的作业部件末端中心的位置信息；

(5)计算卫星天线至作业部件末端中心的坐标增量；

(6)结合步骤(5)及卫星天线的位置信息，计算作业部件末端中心的三维坐标；

或步骤e中，根据激光传感的位置信息、车体姿态信息和各执行机构的姿态信息及位置变化量，建立激光高程解算模型，求解作业部件末端中心的高程信息，激光高程解算模型如下：

其中，n＝1,2,...；H表示作业部件末端中心相对于基准作业面的相对位置信息；h_p表示激光接收器相对于基准作业面的相对高程；ψ,θ分别为车体偏航角、横滚角和俯仰角；β₁,β₂,......,β_n为各执行机构上姿态传感器测量值；l₁,l₂,......,l_n为各执行机构上位移传感器测量值；

作业部件末端中心高程解算步骤如下：

(1)测量车体及执行机构的角度与长度信息和激光接收器至车体或载体平台姿态传感器安装位置的Z轴坐标增量，输入激光高程解算模型；

(2)计算车体姿态旋转矩阵；

(3)根据几何关系或机器人运动学及所测量的各执行机构姿态信息与位置变化量，解算基于车体坐标系的作业部件末端中心的坐标Z轴分量；

(4)计算激光接收器至作业部件末端中心的Z轴坐标增量；

(5)结合步骤(4)及激光传感的位置信息，计算作业部件末端中心相对于基准作业面的相对位置信息。

2.按照权利要求1所述的一种基于车体位姿的作业部件末端位姿测量方法，其特征在于：步骤b中，GNSS系统用于计算偏航角ψ及空间位置坐标。

3.按照权利要求1所述的一种基于车体位姿的作业部件末端位姿测量方法，其特征在于：步骤d中，根据执行机构的运动需求，传感器选择姿态传感器或位移传感器。