专利摘要

一种自主移动机构法矢检测与调姿运动方法，其特征是它包括以下步骤：首先，在自主移动机构末端执行器上安装有三个或四个激光测距传感器；其次，利用三个或四个激光测距传感器所测的值求得所测制孔平面法矢n；第三，根据上述测量计算所的制孔平面法矢n通过调姿运动反解算法，得到自主移动机构到达制孔法矢的各腿驱动量，用于控制系统实现调姿运动；最后，根据上述测量计算所的制孔平面法矢n通过调姿运动反解算法，得到自主移动机构到达制孔法矢时，刀具的偏移量，用于控制系统实现刀具偏移纠正。本发明方法简单，易于控制，控制精度高，高效快速。

权利要求

1.一种自主移动机构法矢检测与调姿运动方法，其特征是它包括以下步骤：

首先，在自主移动机构末端执行器上安装有三个或四个激光测距传感器；

其次，利用三个或四个激光测距传感器所测的值求得所测制孔平面法矢n；

第三，根据上述测量计算所的制孔平面法矢n通过调姿运动反解算法，得到自主移动机构到达制孔法矢的各腿驱动量，用于控制系统实现调姿运动；

最后，根据上述测量计算所的制孔平面法矢n通过调姿运动反解算法，得到自主移动机构到达制孔法矢时，刀具的偏移量，用于控制系统实现刀具偏移纠正。

2.根据权利要求1所述的方法，其特征是所述的平面法矢n的确定方法包括利用三个激光测距传感器和利用四个激光测距传感器所测的值确定制孔位置的平面法矢n；

所述的利用三个激光测距传感器所测的值求得的平面法矢n：

n=(l,m,n)^T=C₁B₁×B₁A₁   (1)

其中由矢量关系可得矢量C₁B₁如公式（2）所示，注意矢量的方向，

C₁B₁=C₁C+CB+BB₁

=(|C₁C|-BB₁|)·(0,0,1)^T-|CB|·(0,1,0)^T   (2)

同理由矢量关系可得矢量B₁A₁如公式（3）所示，注意矢量的方向

B₁A₁=B₁B+BA+AA₁

=(|B₁B|-|AA₁|)·(0,0,1)^T+|BA|·(1,0,0)^T   (3)

式(2)、(3)中参数为各个激光传感器的测量值，将式(2)(3)代入公式(1)即可求得所测平面法矢n；

所述的利用四个激光测距传感器所测的值求得的平面法矢n可再由公式（4）求解刀

具点的法向量，与公式（1）的结果取平均值作为所测平面法矢，提高了测量精度。

(l,m,n)^T=D₁C₁×D₁A₁

whereD₁C₁=(|D₁D|-|C₁C|)·(0,0,1)^T-|CD|·(1,0,0)^T   (4)

D₁A₁=(|D₁D|-|A₁A|)·(0,0,1)^T-|AD|·(0,1,0)^T

如果A、B、C、D中有一个偏差很大，说明该处脱离测量范围，则采用其余三点的数据计算法向：

①当A点超范围，则用B、C、D计算，如公式（5）

                         (l,m,n)^T=C₁B₁×C₁D₁   (5)

②当B点超范围，则用A、C、D计算，如公式（4）

③当C点超范围，则用A、B、D计算，如公式（6）

                         (l,m,n)^T=A₁D₁×A₁B₁   (6)

④当D点超范围，则用A、B、C计算，如公式（1）所示；

以上公式中，设自主移动机构动平台的法向量与制孔主轴始终平行，记n₀=(0,0,1)^T，激光测距传感器A、B、C、D，测距测量线的向量方向同制孔主轴平行，也为n₀=(0,0,1)^T，测距传感器ABCD平面平行于自主移动机构框身，且BA与外框架平行，记初始状态下BA方向为(1,0,0)^T，BC与外框架垂直，记初始状态下BC方向为(0,1,0)^T，|A₁A|、|B₁B|、|C₁C|、|D₁D|即为相应传感器测量距离；检测曲面A₁B₁C₁D₁区域近似看成一个平面，法矢检测即测曲面A₁B₁C₁D₁法向量，即平面A₁B₁C₁的法向量；所测制孔法矢，也称为调姿目标法矢，记为n=(l,m,n)^T。

3.根据权利要求1所述的方法，其特征是所述的调姿运动反解算法是指自主移动机构由n₀=(0,0,1)^T调姿到n=(l,m,n)^T状态，反解八个腿升降的位移量和刀具点的偏移量；

自主移动机构调姿时只有3个自由度，其中旋转自由度仅有A、B角两个方向的转动，即为自主移动机构的俯仰与侧滚动作，自主移动机构腿2仅有一平行于外框方向的补偿量，故自主移动机构由n₀=(0,0,1)^T调姿到n=(l,m,n)^T的过程，也可认为自主移动机构经B向摆角 A向摆角 到达的位姿；

根据坐标旋转变换运算，动坐标系O_m与定坐标系O_b的坐标转换矩阵R_bm如式(7)所示；因为空间旋转矩阵为正交矩阵，故R_bm存在逆矩阵，且R_bm^-1=R_bm^T。

$\begin{array}{c}{R}_{\mathrm{bm}}=\left(\begin{array}{ccc}\cos \left(x_b{x}_m\right)& \cos \left(x_b{y}_m\right)& \cos \left(x_b{z}_m\right)\\ \cos \left(y_b{x}_m\right)& \cos \left(y_b{y}_m\right)& \cos \left(y_b{z}_m\right)\\ \cos \left(z_b{x}_m\right)& \cos \left(z_b{y}_m\right)& \cos \left(z_b{z}_m\right)\end{array}\right)\\ =\left(\begin{array}{ccc}{\cos \phi }_y& 0& {\sin \phi }_y\\ {\sin \phi }_x{\sin \phi }_y& {\cos \phi }_x& {-\sin \phi }_x{\cos \phi }_y\\ {-\cos \phi }_x{\sin \phi }_y& {\sin \phi }_x& {\cos \phi }_x{\cos \phi }_y\end{array}\right)\end{array} ---\left(7\right)$

其中

目标法矢n=(l,m,n)^T已由法矢检测算法测出，因此由公式(8)可求出自主移动机构的调姿A、B摆角 即可解出转换矩阵R_bm；

自主移动机构动坐标系相对于定坐标系的位置，记为

                           O_bO_m=(c,d,h₁)^T   (9)

任一点D在定坐标系位置坐标矢量D_b和在动坐标系的位置矢量D_m关系为：

                                        D_b=R_bmD_m+O_bO_m   (10)

                                     D_m=Rb_m^TD_b-R_bm^TO_bO_m   (11)

设点S_i在坐标系O_b-x_by_bz_b和O_m-x_my_mz_m的坐标分别为S_bi=(x_sib,y_sib,z_sib)^T和S_mi=(x_sim,y_sim,z_sim)^T；

                                     S_im=R_bm^TS_ib-R_bm^TO_bO_m   (12)

                                     S_im-S_1m=R_bm^T(S_ib-S_1b)   (13)

                                    S_im=R_bm^T(S_ib-S_1b)+S_1m   (14)

                                  S_im0=S_ib-O_bO_m=S_ib-(c,d,h)^T   (15)

调姿时，固定足足1不升降，将S_1b=(0,0,0)^T；S_1m=(-c,-d,-h₁)^T代入式(14)，可得调姿前后各足偏移量为

                                  ΔS_im=S_im-S_im0=R_bm^TS_ib-S^ib   (16)

由式（16）解得调姿后各足的输入量与补偿量为

$\left\{\begin{array}{c}{\mathrm{\Delta x}}_{\mathrm{pim}}=x_{\mathrm{sib}}{\cos \phi }_y+y_{\mathrm{sib}}{\sin \phi }_x{\sin \phi }_y\\ -z_{\mathrm{sib}}{\cos \phi }_x{\sin \phi }_y-x_{\mathrm{sib}}\\ \Delta y_{\mathrm{pim}}=y_{\mathrm{sib}}{\cos \phi }_x+z_{\mathrm{sib}}{\sin \phi }_x-y_{\mathrm{sib}}\\ \Delta z_{\mathrm{pim}}=x_{\mathrm{sib}}{\sin \phi }_y-y_{\mathrm{sib}}{\sin \phi }_x{\cos \phi }_y\\ +z_{\mathrm{sib}}{\cos \phi }_x{\cos \phi }_y-z_{\mathrm{sib}}\end{array}\right.---\left(17\right)$

其中Δz_pim为自主移动机构到达目标矢量n=(l,m,n)^T时各足的驱动量，Δx_pim、Δy_pim是各足在动平台x,y两个方向的补偿量；

调姿后，刀具点会产生偏移，为保证保证刀尖点不变，需要给出调姿后刀具点X、Y、Z调整量：

刀尖点T，由公式（11）可得

                                       T_m=R_bm^TT_b-R_bm^TO_bO_m   (18)S_1m=R_bm^TS_1b-R_bm^TO_bO_m

因此可得

                                      T_m-S_1m=R_bm^T(T_b-S_1b)   (19)

                                         T_m0=T_b-(c,d,h)^T   (20)

其中S_1b={x_s1b,y_s1b,z_s1b}^T={0,0,0}^T，S_1m={x_s1m,y_s1m,z_s1m}^T={-c,-d,-h₁}^T

                                     ΔT_m=T_m-T_m0=R_bm^TT_b-T_b   (21)

自主移动机构刀具点T绝对坐标值记为T_b={x_Tb,y_Tb,z_Tb}^T，由公式（21）求得刀具点在动坐标系的坐标值，由此可计算调姿后刀具点在X、Y、Z方向动坐标系下补偿量,如公式(22)所示.

$\left\{\begin{array}{c}{\mathrm{\Delta x}}_T=x_{\mathrm{Tb}}{\cos \phi }_y+y_{\mathrm{Tb}}{\sin \phi }_x{\sin \phi }_y\\ -z_{\mathrm{Tb}}{\cos \phi }_x{\sin \phi }_x-x_{\mathrm{Tb}}\\ {\mathrm{\Delta y}}_T=y_{\mathrm{Tb}}{\cos \phi }_x+z_{\mathrm{Tb}}{\sin \phi }_x-y_{\mathrm{Tb}}\\ {\mathrm{\Delta z}}_T=x_{\mathrm{Tb}}{\sin \phi }_y-y_{\mathrm{Tb}}{\sin \phi }_x\cos {\phi }_y\\ +z_{\mathrm{Tb}}{\cos \phi }_x{\cos \phi }_y-z_{\mathrm{Tb}}\end{array}\right.---\left(22\right).$

说明书

技术领域

本发明涉及一种飞机装配的自主移动机构，尤其是一种飞机装配过程中自主移动机构的法矢检测、法矢调姿及刀具补偿方法，具体地说是一种自主移动机构法矢检测与调姿运动方法。

背景技术

目前，飞机结构采用的主要连接方法是机械连接，一架大型飞机上大约有150万～200万个铆钉和螺栓，为了满足现代飞机高寿命的要求，首先要保证机械连接的安全性和可靠性。近年来，为保证飞机装配质量，提高机体的疲劳寿命，实现大批量生产中的低成本和高效率,以B787，A380，C-17等为代表的现代新型大型飞机装配过程中大量采用了自动化装配技术。飞机自动化装配系统的发展主要为两个方向。一种是成本较高的、适用于批量较大产品的大型专用自动化装配系统，如MPAC、VPAC等。另一种是成本较低的、适用于批量较小产品的轻型自动化装配系统，该种系统的发展方向为轻型化、柔性化、模块化，目前其典型代表有基于工业机器臂自动装配系统、柔性轨道自动化装配系统。

基于工业机器臂自动装配系统是利用市场上通用的工业机器臂为设备本体，配合相应的末端执行器组成的自动化装配系统。其具有机体结构成熟，集成实现较为容易，工作灵活等优点，但存在活动范围较小、结构较大和可移动性差的缺点。基于柔性轨道的自动化装配系统，则是以柔性轨道为平台，吸附在表面上，配上相应的末端执行器，完成自动化制孔等工作，如波音公司申请的中国发明专利CN200580025525.X所公开的柔性轨道多轴工具机及方法。但是，这种基于柔性轨道的自动化装配系统也存在结构较大和移动性差的缺点，尤其是，这种系统需要另外安装轨道，对工装的要求较多，工作前期准备时间较长等缺点。

自主移动机构（如图1所示可参见中国专利CN201310030879.X），属于八足并联机构，机构在工作状态下具有并联机构高刚度、高精度、高承载力的特点，可于飞机产品表面行走并进行定位，且具备实现法向调姿的能力，但由于结构复杂，正解算法很难运用于其运动求解的过程中，并且其结构的特殊性，不能应用普适并联机构的算法。鉴于其多应用于曲面蒙皮表面，决定了其运动的复杂性，决定了算法复杂，控制难度大，对此目前尚无理想的解决方法。

发明内容

本发明的目的是针对现有的八足自主移动机构定位和移动难度大的问题，借助于激光测距器，发明一种自主移动机构法矢检测与调姿运动方法。

本发明的技术方案是：

一种自主移动机构法矢检测与调姿运动方法，其特征是它包括以下步骤：

首先，在自主移动机构末端执行器上安装有三个或四个激光测距传感器；

其次，利用三个或四个激光测距传感器所测的值求得所测制孔平面法矢n；

第三，根据上述测量计算所的制孔平面法矢n通过调姿运动反解算法，得到自主移动机构到达制孔法矢的各腿驱动量，用于控制系统实现调姿运动；

最后，根据上述测量计算所的制孔平面法矢n通过调姿运动反解算法，得到自主移动机构到达制孔法矢时，刀具的偏移量，用于控制系统实现刀具偏移纠正。

所述的平面法矢n的确定方法包括利用三个激光测距传感器和利用四个激光测距传感器所测的值确定制孔位置的平面法矢n；

所述的利用三个激光测距传感器所测的值求得的平面法矢n：

n=(l,m,n)^T=C₁B₁×B₁A₁   (1)

其中由矢量关系可得矢量C₁B₁如公式（2）所示，注意矢量的方向，

C₁B₁=C₁C+CB+BB₁

=(|C₁C|-|BB₁|)·(0,0,1)^T-|CB|·(0,1,0)^T   (2)

同理由矢量关系可得矢量B₁A₁如公式（3）所示，注意矢量的方向

B₁A₁=B₁B+BA+AA₁

=(|B₁B|-|AA₁|)·(0,0,1)^T+|BA|·(1,0,0)^T   (3)

式(2)、(3)中参数为各个激光传感器的测量值，将式(2)(3)代入公式(1)即可求得所测平面法矢n；

所述的利用四个激光测距传感器所测的值求得的平面法矢n可再由公式（4）求解刀

具点的法向量，与公式（1）的结果取平均值作为所测平面法矢，提高了测量精度。

(l,m,n)^T=D₁C₁×D₁A₁

whereD₁C₁=(|D₁D|-|C₁C|)·(0,0,1)^T-|CD|·(1,0,0)^T   (4)

D₁A₁=(|D₁D|-|A₁A|)·(0,0,1)^T-|AD|·(0,1,0)^T

如果A、B、C、D中有一个偏差很大，说明该处脱离测量范围（如在边缘上测量或某处有空洞），则采用其余三点的数据计算法向：

①当A点超范围，则用B、C、D计算，如公式（5）

                         (l,m,n)^T=C₁B₁×C₁D₁   (5)

②当B点超范围，则用A、C、D计算，如公式（4）

③当C点超范围，则用A、B、D计算，如公式（6）

                         (l,m,n)^T=A₁D₁×A₁B₁   (6)

④当D点超范围，则用A、B、C计算，如公式（1）所示；

以上公式中，设自主移动机构动平台的法向量与制孔主轴始终平行，记n₀=(0,0,1)^T，激光测距传感器A、B、C、D，测距测量线的向量方向同制孔主轴平行，也为n₀=(0,0,1)^T，测距传感器ABCD平面平行于自主移动机构框身，且BA与外框架平行，记初始状态下BA方向为(1,0,0)^T，BC与外框架垂直，记初始状态下BC方向为(0,1,0)^T，|A₁A|、|B₁B|、|C₁C|、|D₁D|即为相应传感器测量距离；检测曲面A₁B₁C₁D₁区域近似看成一个平面（实际情况一般A₁B₁C₁和A₁B₁D₁是两个平面），法矢检测即测曲面A₁B₁C₁D₁法向量，即平面A₁B₁C₁（和平面A₁B₁D₁）的法向量；所测制孔法矢，也称为调姿目标法矢，记为n=(l,m,n)^T。

所述的调姿运动反解算法是指自主移动机构由n₀=(0,0,1)^T调姿到n=(l,m,n)^T状态，反解八个腿升降的位移量和刀具点的偏移量；

自主移动机构调姿时只有3个自由度，其中旋转自由度仅有A、B角两个方向的转动，即为自主移动机构的俯仰与侧滚动作，自主移动机构腿2仅有一平行于外框方向的补偿量，故自主移动机构由n₀=(0,0,1)^T调姿到n=(l,m,n)^T的过程，也可认为自主移动机构经B向摆角 A向摆角 到达的位姿；

根据坐标旋转变换运算，动坐标系O_m与定坐标系O_b的坐标转换矩阵R_bm如式(7)所示；因为空间旋转矩阵为正交矩阵，故R_bm存在逆矩阵，且R_bm^-1=R_bm^T。

Rbm=cos(xbxm)cos(xbym)cos(xbzm)cos(ybxm)cos(ybym)cos(ybzm)cos(zbxm)cos(zbym)cos(zbzm)=cosφy0sinφysinφxsinφycosφx-sinφxcosφy-cosφxsinφysinφxcosφxcosφy---(7)]]>

其中

目标法矢n=(l,m,n)^T已由法矢检测算法测出，因此由公式(8)可求出自主移动机构的调姿A、B摆角 即可解出转换矩阵R_bm；

自主移动机构动坐标系相对于定坐标系的位置，记为

                           O_bO_m=(c,d,h₁)^T   (9)

任一点D在定坐标系位置坐标矢量D_b和在动坐标系的位置矢量D_m关系为：

                                         D_b=R_bmD_m+O_bO_m   (10)

                                     D_m=R_bm^TD_b-R_bm^TO_bO_m   (11)

设点S_i在坐标系O_b-x_by_bz_b和O_m-x_my_mz_m的坐标分别为S_bi=(x_sib,y_sib,z_sib)^T和S_mi=(x_sim,y_sim,z_sim)^T；

                                      S_im=R_bm^TS_ib-R_bm^TO_bO_m   (12)

                                     S_im-S_1m=R_bm^T(S_ib-S_1b)   (13)

                                     S_im=R_bm^T(S_ib-S_1b)+S_1m   (14)

                                  S_im0=S_ib-O_bO_m=S_ib-(c,d,h)^T   (15)

调姿时，固定足足1不升降，将S_1b=(0,0,0)^T；S_1m=(-c,-d,-h₁)^T代入式(14)，可得调姿前后各足偏移量为

                                  ΔS_im=S_im-S_im0=R_bm^TS_ib-S_ib   (16)

由式（16）解得调姿后各足的输入量与补偿量为

Δxpim=xsibcosφy+ysibsinφxsinφy-zsibcosφxsinφy-xsibΔypim=ysibcosφx+zsibsinφx-ysibΔzpim=xsibsinφy-ysibsinφxcosφy+zsibcosφxcosφy-zsib---(17)]]>

其中Δz_pim为自主移动机构到达目标矢量n=(l,m,n)^T时各足的驱动量，Δx_pim、Δy_pim是各足在动平台x,y两个方向的补偿量；

调姿后，刀具点会产生偏移，为保证保证刀尖点不变，需要给出调姿后刀具点X、Y、Z调整量：

刀尖点T，由公式（11）可得

                                       T_m=R_bm^TT_b-R_bm^TO_bO_m   (18)

                                          S_1m=R_bm^TS_1b-R_bm^TO_bO_m

因此可得

                                      T_m-S_1m=R_bm^T(T_b-S_1b)   (19)

                                         T_m0=T_b-(c,d,h)^T   (20)

其中S_1b={x_s1b,y_s1b,z_s1b}^T={0,0,0}^T，S_1m={x_s1m,y_s1m,z_s1m}^T={-c,-d,-h₁}^T

                                     ΔT_m=T_m-T_m0=R_bm^TT_b-T_b   (21)

自主移动机构刀具点T（加工点）绝对坐标值记为T_b={x_Tb,y_Tb,z_Tb}^T，由公式（21）求得刀具点在动坐标系的坐标值，由此可计算调姿后刀具点在X、Y、Z方向动坐标系下补偿量,如公式(22)所示.

ΔxT=xTbcosφy+yTbsinφxsinφy-zTbcosφxsinφx-xTbΔyT=yTbcosφx+zTbsinφx-yTbΔzT=xTbsinφy-yTbsinφxcosφy+zTbcosφxcosφy-zTb---(22).]]>

本发明的有益效果：

本发明方法简单，易于控制，由安装在自主移动机构末端执行器上的四个激光测距传感器的测量值，可测出自主移动机构平台坐标系下所测蒙皮表面区域的制孔法矢向量。再根据所测蒙皮表面区域的制孔法矢，运用调姿运动反解算法，能很方便地求解自主移动机构到达制孔法矢的各腿驱动量，用于控制系统实现调姿运动。同时根据所测蒙皮表面区域的制孔法矢，运用调姿运动反解算法，还能方便地求解出自主移动机构到达制孔法矢时，刀具的偏移量，用于控制系统实现刀具偏移纠正。因此，具有控制精度高，高效快速的优点。

附图说明

图1是本发明的自主移动机构的立体结构示意图。

图2是本发明的自主移动机构末端执行器上的激光测距传感器结构分布图。

图3是本发明的由多激光测距传感器组成的法矢检测系统示意图。

图4是本发明所涉及的自主移动机构机构简图。

图5是本发明所涉及的自主移动机构调姿运动机构简图。

图1中：1.外框，2.内框，3.腿，4.双偏心补偿机构，5.末端执行器。

图2中：A,B,C,D为四个激光测距传感器，AB平行于外框1，AD垂直于外框1。

图3中：T为刀具点，A₁,B₁,C₁,D₁为各激光测距传感器的测量点，在蒙皮上。

图5中，腿i(i=1,2,…,8）上的球副位置记为S_i(i=1,2,…,8)，其腿上移动副位置记为P_i(i=1,2,…,8)，R_i1、R_i2为双偏心旋转。

具体实施方式

下面结合附图和实施例对本发明作进一步的说明。

如图1-5所示。

一种自主移动机构法矢检测与调姿运动方法，它包括以下步骤：

首先，在自主移动机构末端执行器上安装有三个或四个激光测距传感器；

其次，利用三个或四个激光测距传感器所测的值求得所测制孔平面法矢n；

第三，根据上述测量计算所的制孔平面法矢n通过调姿运动反解算法，得到自主移动机构到达制孔法矢的各腿驱动量，用于控制系统实现调姿运动；

最后，根据上述测量计算所的制孔平面法矢n通过调姿运动反解算法，得到自主移动机构到达制孔法矢时，刀具的偏移量，用于控制系统实现刀具偏移纠正。

详述如下：

自主移动机构动平台的法向量与制孔主轴始终平行，记n₀=(0,0,1)^T，激光测距传感器A、B、C、D（实际3个即可），测距测量线的向量方向同制孔主轴平行，也为n₀=(0,0,1)^T。测距传感器ABCD平面平行于自主移动机构框身，且BA与外框架平行，记初始状态下BA方向为(1,0,0)^T，BC与外框架垂直，记初始状态下BC方向为(0,1,0)^T。|A₁A|、|B₁B|、|C₁C|、|D₁D|即为相应传感器测量距离。

检测曲面A₁B₁C₁D₁区域近似看成一个平面（实际情况一般A₁B₁C₁和A₁B₁D₁是两个平面），法矢检测即测曲面A₁B₁C₁D₁法向量，即平面A₁B₁C₁（和平面A₁B₁D₁）的法向量。

本发明中所测制孔法矢，也称为调姿目标法矢，记为n=(l,m,n)^T。

实际三个激光测距传感器即可求所测平面法矢n

如图3，由向量叉乘可知所测平面法矢n可由公式（1）求得

                          n=(l,m,n)^T=C₁B₁×B₁A₁   (1)

其中由矢量关系可得矢量C₁B₁如公式（2）所示，注意矢量的方向

                                C₁B₁=C₁C+CB+BB₁

              =(|C₁C|-|BB₁|)·(0,0,1)^T-|CB|·(0,1,0)^T   (2)

同理由矢量关系可得矢量B₁A₁如公式（3）所示，注意矢量的方向

                              B₁A₁=B₁B+BA+AA₁

               =(|B₁B|-|AA₁|)·(0,0,1)^T+|BA|·(1,0,0)^T   (3)

式(2)(3)中参数为个激光传感器的测量值，将式(2)(3)代入公式(1)即可求得所测平面法矢n。

本发明法矢检测系统由四个激光测距传感器A、B、C和D，下面给出四传感器下，求得的所测平面法矢n

如果有第四个测距仪D，可再由公式（4）求解刀具点的法向量，与公式（1）的结果取平均值作为所测平面法矢，提高了测量精度。

(l,m,n)^T=D₁C₁×D₁A₁

          whereD₁C₁=(|D₁D|-|C₁C|)·(0,0,1)^T-|CD|·(1,0,0)^T   (4)

                 D₁A₁=(|D₁D|-|A₁A|)·(0,0,1)^T-|AD|·(0,1,0)^T

如果A、B、C、D中有一个偏差很大，说明该处脱离测量范围（如在边缘上测量或某处有空洞），则采用其余三点的数据计算法向。

①当A点超范围，则用B、C、D计算，如公式（5）

                          (l,m,n)^T=C₁B₁×C₁D₁   (5)

②当B点超范围，则用A、C、D计算，如公式（4）

③当C点超范围，则用A、B、D计算，如公式（6）

                          (l,m,n)^T=A₁D₁×A₁B₁   (6)

④当D点超范围，则用A、B、C计算，如公式（1）

测出调姿目标法矢后，求解出到达目标位姿时各腿的驱动量，下面结合附图，说明调姿运动算法。

如图4，图5所示。

法向调姿过程中，自主移动机构八条腿吸紧，外框1与内框2锁死相对固定，调姿机构简图如图5所示。框体(包括外框1，内框2，末端执行器5)视为动平台，机身表面视为定平台，八条腿为连接动定平台的支链，八条腿的移动副为驱动副。通过八退腿伸长量实现自主移动机构的俯仰与侧滚，从而完成调姿动作，使自主移动机构的制孔法矢与目标法矢n重合，调姿过程中，各腿3末端产生的偏移量由双偏心补偿机构4来补偿。

建立如图5所示右手正交坐标系。定坐标系：O_b-x_by_bz_b，即固定于蒙皮上的坐标系，原点O_b位于S₁上，x_b为初始状态平行于自主移动机构外框的BA方向，y_b为初始状态平行于自主移动机构的BC方向，z_b为x_b和y_b确定的右手坐标系，即初始状态下自主移动机构的制孔法矢为n₀=(0,0,1)^T。动坐标系：O_m-x_my_mz_m，即固定于自主移动机构的坐标系，原点O_m为P₁，z_m与腿的升降方向平行，x_m始终平行于外框BA方向，y_b始终平行于BC方向。初始状态下动坐标系与定坐标系平行。

本发明所涉及的调姿运动算法，可以描述为自主移动机构由n₀=(0,0,1)^T调姿到n=(l,m,n)^T状态，反解八个腿升降的位移量。

自主移动机构调姿时只有3个自由度，其中旋转自由度仅有A、B角两个方向的转动，即为自主移动机构的俯仰与侧滚动作。如图5所示，自主移动机构腿2仅有一平行于外框方向的补偿量，故自主移动机构由n₀=(0,0,1)^T调姿到n=(l,m,n)^T的过程，也可认为自主移动机构经B向摆角 A向摆角 到达的位姿。

根据坐标旋转变换运算，动坐标系O_m与定坐标系O_b的坐标转换矩阵R_bm如式(7)所示。因为空间旋转矩阵为正交矩阵，故R_bm存在逆矩阵，且R_bm^-1=R_bm^T。

Rbm=cos(xbxm)cos(xbym)cos(xbzm)cos(ybxm)cos(ybym)cos(

自主移动机构法矢检测与调姿运动方法专利购买费用说明

Q：办理专利转让的流程及所需资料

A：专利权人变更需要办理著录项目变更手续，有代理机构的，变更手续应当由代理机构办理。

1：专利变更应当使用专利局统一制作的“著录项目变更申报书”提出。

2：按规定缴纳著录项目变更手续费。

3：同时提交相关证明文件原件。

4：专利权转移的，变更后的专利权人委托新专利代理机构的，应当提交变更后的全体专利申请人签字或者盖章的委托书。

Q:专利著录项目变更费用如何缴交

A：（1）直接到国家知识产权局受理大厅收费窗口缴纳,（2）通过代办处缴纳，（3）通过邮局或者银行汇款,更多缴纳方式

Q：专利转让变更，多久能出结果

A：著录项目变更请求书递交后，一般1-2个月左右就会收到通知，国家知识产权局会下达《转让手续合格通知书》。