[发明专利]一种柔性体空间形状和位姿感知方法及系统

权利要求书

1.一种柔性体空间形状和位姿感知方法，其特征在于：包括以下过程：

获取与柔性体连接且被划分为多节的柔性载体的传感数据；

根据获取的传感数据，得到柔性载体每节的伸缩应变、偏转角、曲率、弯曲角和扭转角；

根据得到的偏转角、曲率、弯曲角、扭转角和预设运动学模型，得到柔性载体各节位姿参数；

根据柔性载体各节位姿参数，得到柔性体空间形状和位姿感知结果；

根据各节及相邻节的旋转变换矩阵和平移变换矩阵得到柔性载体固定端坐标系和运动端坐标系之间的变换矩阵；

根据得到的变换矩阵得到柔性载体运动端中心的空间坐标和运动端端面的欧拉角，进而重构出柔性体的空间形状和位姿；

由基坐标系{A}至第一节起点{10}的变换矩阵为：

；

其中，为第一节所在平面的偏转角，为第一节所在平面的扭转角，为第一节由末端非线性形变引起的被动偏角，为绕基坐标系{A}的z轴旋转由基坐标系{A}至坐标系{B₁}的旋转矩阵，为绕坐标系{B₁}的x轴旋转由坐标系{B₁}至坐标系{C₁}的旋转矩阵，为绕坐标系{C₁}的y轴旋转由坐标系{C₁}至第一节起点坐标系{10}的旋转矩阵；

任意节起点坐标系{i0}至末点坐标系{i}的变换矩阵为：

其中，为第i节所在平面的弯曲角，为第i节所在平面的曲率，为绕起点坐标系{i0}的y轴旋转由起点坐标系{i0}至末点坐标系{i}的旋转矩阵，为由起点坐标系{i0}平移至末点坐标系{i}的平移矩阵；

第i-1节末点坐标系{i-1}至第i节起点坐标系{i0}的变换矩阵为：

其中，为第i-1节末点坐标系{i-1}至中转坐标系{i0^’}的夹角，为中转坐标系{i0^’}至第i节起点坐标系{i0}的夹角，为绕坐标系{i-1}的z轴旋转由第i-1节末点坐标系{i-1}至中转坐标系{i0^’}的旋转矩阵，为绕中转坐标系{i0^’}的x轴旋转由中转坐标系{i0^’}至第i节起点坐标系{i0}的旋转矩阵；

第i-1节末点坐标系{i-1}至第i节起点坐标系{i0}的变换矩阵为：

其中，为第i-1节末点坐标系{i-1}至中转坐标系{i0^’}的夹角，为中转坐标系{i0^’}至第i节起点坐标系{i0}的夹角，为绕坐标系{i-1}的z轴旋转由第i-1节末点坐标系{i-1}至中转坐标系{i0^’}的旋转矩阵，为绕中转坐标系{i0^’}的x轴旋转由中转坐标系{i0^’}至第i节起点坐标系{i0}的旋转矩阵；

基坐标系{A}至第i节末端的坐标变换矩阵为：

其中，(x_i,y_i,z_i)是第i节末端中心的空间坐标，(α_i,β_i, γ_i)是由基坐标至第i节末端中心坐标的欧拉角。