一种基于RTK的高空作业车定位定姿方法与流程

一种基于rtk的高空作业车定位定姿方法
技术领域
1.本发明涉及作业车定位技术领域，具体为一种基于rtk的高空作业车定位定姿方法。


背景技术：

2.公开号为cn114798253a的专利一种高空作业的设备及其控制方法，提供了一种高空作业设备，此设备可以基于钢结构厂房的3d模型实现自动喷涂钢结构。
3.由于厂房的3d模型已知，可以根据厂房的3d模型建立一个直角坐标系称为厂房坐标系，从而待喷涂钢结构部件在厂房坐标系中的位置和姿态是已知的，对应到喷涂机械臂末端移动的路径在厂房坐标系中是已知的。因此，实现喷涂机械臂按预先设定的喷涂轨迹运动的必要条件是确定机械臂的基座在厂房坐标系中停放的位置和姿态。另外，为实现控制高空作业车大臂将机械臂送至喷涂厂房坐标系中某一位置，需要确定高空作业车底盘在厂房坐标系中停靠的位置和姿态，要保证喷涂作业效率，定位定姿的时间必须做到实时，手动测量的方法不可行；由于钢结构厂房中存在檩条等较小部件，定位定姿的误差必须足够小，否则会导致喷涂表面无法被完整覆盖。因此就需要一种定位定姿方法。


技术实现要素：

4.鉴于现有技术中所存在的问题，本发明公开了一种基于rtk的高空作业车定位定姿方法，包括步骤如下：步骤一、将rtk的测量值转换为厂房直角坐标：实现该转换可分为以下两步:步骤1、将大地测量坐标转换为地心地固直角坐标在wgs-84坐标系统中使用以下大地测量常数来计算直角坐标，地球的长半轴a＝6378137米，地球的偏心率平方通过下面的公式将纬度，经度和高程转换为地心地固直角坐标系(x，y，z)转换为地心地固直角坐标系(x，y，z)转换为地心地固直角坐标系(x，y，z)步骤2、确定地心地固坐标系和厂房坐标系间的刚性变换由于地心地固坐标系与厂房坐标系均为直角坐标系，且长度单位均为米，因此两个坐标系间存在确定的刚性变换r，t使得pa＝r
·epa+t，其中pa为点a在厂房坐标系中的坐
标，epa为点a在地心地固坐标系中的坐标，r∈so(3)为旋转矩阵，为平移向量；为确定刚性变换的参数r，t，在厂房中选取若干固定的点q1，q2，...，qn，在每个点利用rtk测量出该点在地心地固坐标系中的坐标p1，p2，...，pn，则参数r，t的选取应满足误差距离的平方和最小满足上述条件的参数可以通过以下步骤求解：(1)、计算两组点集的质心(2)、计算两组中心化的向量(3)、计算3
×
3协方差矩阵s＝xy
t
，其中x＝[x1ꢀ…ꢀ
xn]，y＝[y1ꢀ…ꢀyn
](4)、计算s的奇异值分解s＝u∑v
t
，则最优旋转矩阵为(5)、最优平移向量为因此基于以上两步可以将rtk的测量值直接变换到厂房坐标系下，从而后面部分都假定rtk直接输出厂房坐标系下的x，y，z坐标；步骤二、确定高空作业车在厂房坐标系中的位置和姿态为了测量出高空作业车在厂房坐标系中的位置和姿态，在车底盘上安装两个rtk测量点。当厂房地面较平整时，可以假设车底盘与厂房地面平行，那么只利用两个rtk测量点就可以确定高空作业车的位置和姿态，方法如下：假设两个rtk在高空作业车中的安装位置分别为cpa和cpb已知同时根据rtk的测量值可以知道两点在厂房中的位置pa和pb根据这两对测量点确定高空作业车在厂房中的位置pc和姿态rc；首先有以下关系pa＝rc·cpa+pcꢀꢀ
(1)pb＝rc·cpb+pcꢀꢀ
(2)如果车底盘与地面平行，即高空作业车的z轴与厂房的z轴一致，则旋转矩阵rc的列向量可以表示为：rc＝[r
1 r
2 e3]由列向量的正交性可得《r1，e3》＝《r2，e3》＝0，从而
由(1)，(2)式相减得到：p
b-pa＝rc·
(cp
b-c
pa)写成坐标的形式：只考虑前两项得到：方程左边的矩阵是一个2d旋转矩阵，因此可以写为：从而得到形式如下的方程组：c1cosθ+c2sinθ+c3＝0c1cosθ-c2sinθ+c4＝0有唯一解θ＝arctan2(-c1c
4-c2c3，c2c
4-c1c3)；计算高空作业车的姿态后，将(1)，(2)式相加得到高空作业车的位置如果车底盘与地面不平行，为确定高空作业车的准确位姿不能假设车的z轴方向与厂房的z轴方向一致，在车底盘上安装倾角传感器，结合rtk测量点的坐标来确定高空作业车的精确位姿；倾角传感器测量的是传感器的x，y轴与海平面的夹角，即与重力方向夹角的余角；如果在安装倾角传感器时使得传感器的x，y轴与高空作业车的x，y轴重合，则可以测量出高空作业车x，y轴与重力方向的夹角；由于重力方向与厂房的z轴的反方向近似一致，因此，通过倾角传感器实际上可以测量出高空作业车x，y轴与厂房z轴的夹角；假设倾角传感器在x，y轴方向的读数分别为α
x
，αy，因此重力方向上的单位向量在高空作业车坐标系中可以表示为：而重力方向上的单位向量在厂房坐标系中的坐标为：因此，假设高空作业车在厂房中的位置为pc和姿态为rc，结合两个rtk在高空作业
车中的安装位置cpa和cpb，和在厂房中测量位置pa和pb，一共得到三组对应关系：pg＝rc·cpg+pcꢀꢀ
(1)pa＝rc·cpa+pcꢀꢀ
(2)pb＝rc·cpb+pcꢀꢀ
(3)(2)，(3)与(1)相减得到p
a-pg＝rc·
(cp
a-c
pg)p
b-pg＝rc·
(cp
b-c
pg)记及将单位正交化：位正交化：从而得到单位正交向量对做同样的线性组合得到：线性组合得到：因此因此由于rc是旋转矩阵，因此同样是单位正交向量，这样便得到：从而计算高空作业车的姿态后，将(1)，(2)，(3)式相加得到高空作业车的位置：
[0005]
根据上面的推导容易看出，倾角传感器的读数实质上确定了一个三维向量在高空作业车坐标系和厂房坐标系中的对应关系，即上节中的cpg和pg的对应关系。因此如果不使用倾角传感器，利用高空作业车末端平台的rtk测量点同样可以确定一个三维向量(即点的坐标)在高空作业车坐标系和厂房坐标系中的对应关系，由于开始作业前高空作业车末端平台的位置在零位处，因此平台上rtk的位置在高空作业车的坐标系中的坐标是固定的，如果能够测量出该位置在高空作业车中的坐标，结合高空作业车车底盘上两个rtk的测量点，同样可以确定高空作业车的位姿。
[0006]
这样便给出了只利用三个rtk测量点便可确定高空作业车在厂房中位姿的方法。
[0007]
综合上述讨论，确定高空作业车在厂房中位姿的方法有三种，总结如下：(1)、对于厂房地面平整的场景只利用车底盘上安装的两个rtk测量点便可确定高
空作业车在厂房中的位姿；(2)、对于厂房地面不平整的场景，有两种改进方法：1)、在车底盘上安装倾角传感器，根据两个rtk测量点和倾角传感器的读数确定高空作业车在厂房中的位姿；2)、无需加装倾角传感器，利用末端平台上安装的rtk测量点，只需保证作业前大臂保持在零位状态固定位置，根据三个rtk测量点同样可以确定高空作业车在厂房中的位姿；步骤三、确定大臂末端平台在厂房坐标系中的位置和姿态末端平台在厂房中的位置可以根据平台上安装的rtk直接测量得到，为计算平台的姿态可以利用高空作业车的运动学模型及已经计算出的高空作业车在厂房中的位姿。由于末端平台总是近乎保持与车底盘平行，因此高空作业车大臂只包含三个自由度：旋转、俯仰和伸缩。根据末端平台中某点在高空作业车的位置可以反解出大臂各关节值，从而利用正运动学计算出平台在高空作业车中的位姿，最后利用高空作业车在厂房中的位姿计算出末端平台在厂房中的位姿。使用标准d-h参数对大臂运动学建模如下：末端平台在高空作业车中的位姿由三个关节值θ1，θ2，d3唯一确定，其余参数ai，di由实际测量大臂的尺寸得到；现根据末端平台rtk的测量值计算出三个关节值θ1，θ2，d3；假设末端平台rtk在厂房坐标系中测量的点为p
p
，高空作业车在厂房中的位姿用4
×
4齐次矩阵表示如下：则可以计算出末端平台rtk在高空作业车中的位置坐标：根据大臂的d-h参数可以知道各个连杆间的变换矩阵：h参数可以知道各个连杆间的变换矩阵：
从而末端平台相对高空作业车基坐标系的变换矩阵为：t＝a1a2a3a4假设rtk在末端平台安装位置固定且坐标为[x
4 y
4 z4]
t
，前面计算出rtk在高空作业车基坐标系中的坐标为cp
p
＝[x
0 y
0 z0]
t
，那么有：上述等式等价于：计算变换a1，a2的逆如下：等号左手边式子为：等号右边的式子为：首先，第二个等式形如c1cosθ1+c2sinθ1+c3＝0方程，可以求解出θ1的值，将θ1的值代入第一个等式得到形如c4cosθ2+c5sinθ2+c6＝0方程，从而可以求解出θ2的值，最后根据第三个等式求出d3的值；根据rtk的测量值可以求出大臂的三个关节值，从而计算末端平台相对于高空作业车的位姿，再利用前面计算的高空作业车相对厂房的位姿可以求出末端平台在厂房中的位姿。
[0008]
本发明的有益效果：本发明根据高空作业车的工作环境及工作要求，提供了一种确定高空作业车机械臂的基座在厂房中的位置和姿态的方法。结合需要喷涂的厂房的3d模型建立厂房坐标系，明确需要喷涂的钢结构部件的位置坐标，根据已知坐标确定喷涂机械臂末端的移动路径，从而便于高空作业车自动化喷涂作业，能够实时对作业车进行定位定
姿，保证了喷涂作业效率，同时，定位定姿误差小，可以保证喷涂作业全覆盖，不会遗漏喷涂面。
具体实施方式
[0009]
实施例1
[0010]
本发明的一种基于rtk的高空作业车定位定姿方法，包括步骤如下：步骤一、将rtk的测量值转换为厂房直角坐标rtk是
″
real-time kinematic
″
的缩写，是一种提供实时、厘米级精度定位的技术。由于rtk测量值为大地测量坐标系中的经度、纬度和高程值，无法直接应用到厂房坐标系下，因此需要先将rtk的测量值转换为厂房坐标系中。实现该转换可分为以下两步：步骤1、将大地测量坐标转换为地心地固直角坐标在wgs-84坐标系统中使用以下大地测量常数来计算直角坐标，地球的长半轴a＝6378137米，地球的偏心率平方通过下面的公式将纬度，经度和高程转换为地心地固直角坐标系(x，y，z)转换为地心地固直角坐标系(x，y，z)转换为地心地固直角坐标系(x，y，z)步骤2、确定地心地固坐标系和厂房坐标系间的刚性变换由于地心地固坐标系与厂房坐标系均为直角坐标系，且长度单位均为米，因此两个坐标系间存在确定的刚性变换r，t使得pa＝r
·epa+t，其中pa为点a在厂房坐标系中的坐标，epa为点a在地心地固坐标系中的坐标，r∈so(3)为旋转矩阵，为平移向量；为确定刚性变换的参数r，t，在厂房中选取若干固定的点q1，q2，...，qn，在每个点利用rtk测量出该点在地心地固坐标系中的坐标p1，p2，...，pn，则参数r，t的选取应满足误差距离的平方和最小满足上述条件的参数可以通过以下步骤求解：(1)、计算两组点集的质心(2)、计算两组中心化的向量(3)、计算3
×
3协方差矩阵s＝xy
t
，其中x＝[x1ꢀ…ꢀ
xn]，y＝[y1ꢀ…ꢀyn
](4)、计算s的奇异值分解s＝u∑v
t
，则最优旋转矩阵为
(5)、最优平移向量为完成将rtk的测量值直接变换到厂房坐标系下，从而后面部分都假定rtk直接输出厂房坐标系下的x，y，z坐标；步骤二、确定高空作业车在厂房坐标系中的位置和姿态在车底盘上安装两个rtk测量点，利用两个rtk测量点确定高空作业车的位置和姿态，方法如下：假设两个rtk在高空作业车中的安装位置分别为cpa和cpb已知同时根据rtk的测量值可以知道两点在厂房中的位置pa和pb根据这两对测量点确定高空作业车在厂房中的位置pc和姿态rc；首先有以下关系pa＝rc·cpa+pcꢀꢀ
(1)pb＝rc·cpb+pcꢀꢀ
(2)如果车底盘与地面平行，即高空作业车的z轴与厂房的z轴一致，则旋转矩阵rc的列向量可以表示为：rc＝[r
1 r
2 e3]由列向量的正交性可得《r1，e3》＝《r2，e3》＝0，从而由(1)，(2)式相减得到：p
b-pa＝rc·
(cp
b-c
pa)写成坐标的形式：只考虑前两项得到：方程左边的矩阵是一个2d旋转矩阵，因此可以写为：
从而得到形式如下的方程组：c1cosθ+c2sinθ+c3＝0c1cosθ-c2sinθ+c4＝0有唯一解θ＝arctan2(-c1c
4-c2c3，c2c
4-c1c3)；计算高空作业车的姿态后，将(1)，(2)式相加得到高空作业车的位置如果车底盘与地面不平行，为确定高空作业车的准确位姿不能假设车的z轴方向与厂房的z轴方向一致，在车底盘上安装倾角传感器，结合rtk测量点的坐标来确定高空作业车的精确位姿；倾角传感器测量的是传感器的x，y轴与海平面的夹角，即与重力方向夹角的余角；如果在安装倾角传感器时使得传感器的x，y轴与高空作业车的x，y轴重合，则可以测量出高空作业车x，y轴与重力方向的夹角；由于重力方向与厂房的z轴的反方向近似一致，因此，通过倾角传感器实际上可以测量出高空作业车x，y轴与厂房z轴的夹角；假设倾角传感器在x，y轴方向的读数分别为α
x
，αy，因此重力方向上的单位向量在高空作业车坐标系中可以表示为：而重力方向上的单位向量在厂房坐标系中的坐标为：因此，假设高空作业车在厂房中的位置为pc和姿态为rc，结合两个rtk在高空作业车中的安装位置cpa和cpb，和在厂房中测量位置pa和pb，一共得到三组对应关系：pg＝rc·cpg+pcꢀꢀ
(1)pa＝rc·cpa+pcꢀꢀ
(2)pb＝rc·cpb+pcꢀꢀ
(3)(2)，(3)与(1)相减得到p
a-pg＝rc·
(cp
a-c
pg)p
b-pg＝rc·
(cp
b-c
pg)记及将单位正交化：位正交化：
从而得到单位正交向量对做同样的线性组合得到：线性组合得到：因此因此由于rc是旋转矩阵，因此同样是单位正交向量，这样便得到：从而计算高空作业车的姿态后，将(1)，(2)，(3)式相加得到高空作业车的位置：根据上面的推导容易看出，倾角传感器的读数实质上确定了一个三维向量在高空作业车坐标系和厂房坐标系中的对应关系，即上节中的cpg和pg的对应关系。因此如果不使用倾角传感器，利用高空作业车末端平台的rtk测量点同样可以确定一个三维向量(即点的坐标)在高空作业车坐标系和厂房坐标系中的对应关系，由于开始作业前高空作业车末端平台的位置在零位处，因此平台上rtk的位置在高空作业车的坐标系中的坐标是固定的，如果能够测量出该位置在高空作业车中的坐标，结合高空作业车车底盘上两个rtk的测量点，同样可以确定高空作业车的位姿。这样便给出了只利用三个rtk测量点便可确定高空作业车在厂房中位姿的方法。综合上述讨论，我们给出了三种确定高空作业车在厂房中位姿的方法，可以总结如下：(1)、对于厂房地面平整的场景只利用车底盘上安装的两个rtk测量点便可确定高空作业车在厂房中的位姿；(2)对于厂房地面不平整的场景，有两种改进方法：1)、在车底盘上安装倾角传感器，根据两个rtk测量点和倾角传感器的读数确定高空作业车在厂房中的位姿；2)、无需加装倾角传感器，利用末端平台上安装的rtk测量点，只需保证作业前大臂保持在零位状态固定位置，根据三个rtk测量点同样可以确定高空作业车在厂房中的位姿；步骤三、确定大臂末端平台在厂房坐标系中的位置和姿态末端平台在厂房中的位置可以根据平台上安装的rtk直接测量得到，为计算平台的姿态可以利用高空作业车的运动学模型及已经计算出的高空作业车在厂房中的位姿。由于末端平台总是近乎保持与车底盘平行，因此高空作业车大臂只包含三个自由度：旋转、俯仰和伸缩。根据末端平台中某点在高空作业车的位置可以反解出大臂各关节值，从而利用
正运动学计算出平台在高空作业车中的位姿，最后利用高空作业车在厂房中的位姿计算出末端平台在厂房中的位姿。使用标准d-h参数对大臂运动学建模如下：末端平台在高空作业车中的位姿由三个关节值θ1，θ2，d3唯一确定，其余参数ai，di由实际测量大臂的尺寸得到；现根据末端平台rtk的测量值计算出三个关节值θ1，θ2，d3；假设末端平台rtk在厂房坐标系中测量的点为p
p
，高空作业车在厂房中的位姿用4
×
4齐次矩阵表示如下：则可以计算出末端平台rtk在高空作业车中的位置坐标：根据大臂的d-h参数可以知道各个连杆间的变换矩阵：h参数可以知道各个连杆间的变换矩阵：h参数可以知道各个连杆间的变换矩阵：h参数可以知道各个连杆间的变换矩阵：从而末端平台相对高空作业车基坐标系的变换矩阵为：t＝a1a2a3a4假设rtk在末端平台安装位置固定且坐标为[x
4 y
4 z4]
t
，前面计算出rtk在高空作业车基坐标系中的坐标为cp
p
＝[x
0 y
0 z0]
t
，那么有：
上述等式等价于：计算变换a1，a2的逆如下：等号左手边式子为：等号右边的式子为：首先，第二个等式形如c1cosθ1+c2sinθ1+c3＝0方程，可以求解出θ1的值，将θ1的值代入第一个等式得到形如c4cosθ2+c5sinθ2+c6＝0方程，从而可以求解出θ2的值，最后根据第三个等式求出d3的值；
[0011]
根据rtk的测量值可以求出大臂的三个关节值，从而计算末端平台相对于高空作业车的位姿，再利用前面计算的高空作业车相对厂房的位姿可以求出末端平台在厂房中的位姿。
[0012]
本文中未详细说明的部件为现有技术。
[0013]
上述虽然对本发明的具体实施例作了详细说明，但是本发明并不限于上述实施例，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下做出各种变化，而不具备创造性劳动的修改或变形仍在本发明的保护范围以内。

技术特征：
1.一种基于rtk的高空作业车定位定姿方法，其特征在于，包括步骤如下：步骤一、将rtk的测量值转换为厂房直角坐标步骤1、将大地测量坐标转换为地心地固直角坐标在wgs-84坐标系统中使用以下大地测量常数来计算直角坐标，地球的长半轴a＝6378137米，地球的偏心率平方通过下面的公式将纬度，经度和高程转换为地心地固直角坐标系(x，y，z)转换为地心地固直角坐标系(x，y，z)转换为地心地固直角坐标系(x，y，z)步骤2、确定地心地固坐标系和厂房坐标系间的刚性变换由于地心地固坐标系与厂房坐标系均为直角坐标系，且长度单位均为米，因此两个坐标系间存在确定的刚性变换r，t使得p
a
＝r
·
e
p
a
+t，其中p
a
为点a在厂房坐标系中的坐标，
e
p
a
为点a在地心地固坐标系中的坐标，r∈so(3)为旋转矩阵，为平移向量；为确定刚性变换的参数r，t，在厂房中选取若干固定的点q1，q2，...，q
n
，在每个点利用rtk测量出该点在地心地固坐标系中的坐标p1，p2，...，p
n
，则参数r，t的选取应满足误差距离的平方和最小满足上述条件的参数可以通过以下步骤求解：(1)、计算两组点集的质心(2)、计算两组中心化的向量(3)、计算3
×
3协方差矩阵s＝xy
t
，其中x＝[x1…
x
n
]，y＝[y1…
y
n
](4)、计算s的奇异值分解s＝u∑v
t
，则最优旋转矩阵为(5)、最优平移向量为完成将rtk的测量值直接变换到厂房坐标系下，从而后面部分都假定rtk直接输出厂房坐标系下的x，y，z坐标；步骤二、确定高空作业车在厂房坐标系中的位置和姿态在车底盘上安装两个rtk测量点，利用两个rtk测量点确定高空作业车的位置和姿态，方法如下：
假设两个rtk在高空作业车中的安装位置分别为
c
p
a
和
c
p
b
已知同时根据rtk的测量值可以知道两点在厂房中的位置p
a
和p
b
根据这两对测量点确定高空作业车在厂房中的位置p
c
和姿态r
c
；首先有以下关系p
a
＝r
c
·
c
p
a
+p
c
ꢀꢀ
(1)p
b
＝r
c
·
c
p
b
+p
c
ꢀꢀ
(2)如果车底盘与地面平行，即高空作业车的z轴与厂房的z轴一致，则旋转矩阵r
c
的列向量可以表示为：r
c
＝[r
1 r
2 e3]由列向量的正交性可得<r1，e3)＝<r2，e3>＝0，从而由(1)，(2)式相减得到：p
b-p
a
＝r
c
·
(
c
p
b-c
p
a
)写成坐标的形式：只考虑前两项得到：方程左边的矩阵是一个2d旋转矩阵，因此可以写为：从而得到形式如下的方程组：c1cosθ+c2sinθ+c3＝0c1cosθ-c2sinθ+c4＝0有唯一解θ＝arctan2(-c1c
4-c2c3，c2c
4-c1c3)；计算高空作业车的姿态后，将(1)，(2)式相加得到高空作业车的位置如果车底盘与地面不平行，为确定高空作业车的准确位姿不能假设车的z轴方向与厂房的z轴方向一致，在车底盘上安装倾角传感器，结合rtk测量点的坐标来确定高空作业车的精确位姿；假设倾角传感器在x，y轴方向的读数分别为α
x
，α
y
，因此重力方向上的单位向量在高空作业车坐标系中可以表示为：
而重力方向上的单位向量在厂房坐标系中的坐标为：因此，假设高空作业车在厂房中的位置为p
c
和姿态为r
c
，结合两个rtk在高空作业车中的安装位置
c
p
a
和
c
p
b
，和在厂房中测量位置p
a
和p
b
，一共得到三组对应关系：p
g
＝r
c
·
c
p
g
+p
c
ꢀꢀ
(1)p
a
＝r
c
·
c
p
a
+p
c
ꢀꢀ
(2)p
b
＝r
c
·
c
p
b
+p
c
ꢀꢀ
(3)(2)，(3)与(1)相减得到p
a-p
g
＝r
c
·
(
c
p
a-cp
g
)p
b-p
g
＝r
c
·
(
c
p
b-c
p
g
)记及将单位正交化：化：从而得到单位正交向量对做同样的线性组合得到：合得到：因此因此由于r
c
是旋转矩阵，因此同样是单位正交向量，这样便得到：从而计算高空作业车的姿态后，将(1)，(2)，(3)式相加得到高空作业车的位置：步骤三、确定大臂末端平台在厂房坐标系中的位置和姿态使用标准d-h参数对大臂运动学建模如下：
末端平台在高空作业车中的位姿由三个关节值θ1，θ2，d3唯一确定，其余参数a
i
，d
i
由实际测量大臂的尺寸得到；现根据末端平台rtk的测量值计算出三个关节值θ1，θ2，d3；假设末端平台rtk在厂房坐标系中测量的点为p
p
，高空作业车在厂房中的位姿用4
×
4齐次矩阵表示如下：则可以计算出末端平台rtk在高空作业车中的位置坐标：根据大臂的d-h参数可以知道各个连杆间的变换矩阵：h参数可以知道各个连杆间的变换矩阵：h参数可以知道各个连杆间的变换矩阵：h参数可以知道各个连杆间的变换矩阵：从而末端平台相对高空作业车基坐标系的变换矩阵为：t＝a1a2a3a4假设rtk在末端平台安装位置固定且坐标为[x
4 y
4 z4]
t
，前面计算出rtk在高空作业车基坐标系中的坐标为
c
p
p
＝[x
0 y
0 z0]
t
，那么有：上述等式等价于：计算变换a1，a2的逆如下：
等号左手边式子为：等号右边的式子为：首先，第二个等式形如c1cosθ1+c2sinθ1+c3＝0方程，可以求解出θ1的值，将θ1的值代入第一个等式得到形如c4cosθ2+c5sinθ2+c6＝0方程，从而可以求解出θ2的值，最后根据第三个等式求出d3的值；根据rtk的测量值可以求出大臂的三个关节值，从而计算末端平台相对于高空作业车的位姿，再利用前面计算的高空作业车相对厂房的位姿可以求出末端平台在厂房中的位姿。

技术总结
本发明涉及一种基于RTK的高空作业车定位定姿方法。本发明根据高空作业车的工作环境及工作要求，提供了一种确定高空作业车机械臂的基座在厂房中的位置和姿态的方法。结合需要喷涂的厂房的3D模型建立厂房坐标系，明确需要喷涂的钢结构部件的位置坐标，根据已知坐标确定喷涂机械臂末端的移动路径，从而便于高空作业车自动化喷涂作业，能够实时对作业车进行定位定姿，保证了喷涂作业效率，同时，定位定姿误差小，可以保证喷涂作业全覆盖，不会遗漏喷涂面。不会遗漏喷涂面。


技术研发人员：高艳龙 高玉涛 黄世龙 胡永强
受保护的技术使用者：北京克莱明科技有限公司
技术研发日：2023.08.07
技术公布日：2023/10/11