问题是这样的：
输入：
房间：正方形=> 10米x 10米
6个激光位置：固定在框架上
6个激光方向：固定在框架上
激光的6次测量结果
我的飞行控制器（PixHawk）的IMU产生的四元数。
原点以多旋翼的重心为中心，并且就像壁在垂直于每个轴（X的壁的法线为（-1,0,0））
输出：
3D的位置（X， Y，Z）
角位置（四元数）
由于获得了多转子的角位置，所以我通过四元数旋转了激光的位置和方向，然后通过6次测量进行推断我得到了三堵墙。 （墙壁的方向是微不足道的，那么只有一个点就可以确定其位置。
坏的是，我注意到PixHawk的偏航（围绕z旋转）测量是不可靠的。那么我应该测量激光的偏航，但我没有成功。如果2D问题很容易，我就会迷失在3D中。
有人知道吗[知道XYZ位置和四元数的算法从6个测量值开始]存在某处吗？或解决此问题的正确方法是什么？
问题：我如何从2台激光器的2次测量中获得偏航，我知道其原始位置，方向
注意：绿色指针是原点位置，红色指针是“最终”位置，但可以绕红色圆圈旋转（由于偏航）。
#1 楼
解决方案：还有没有预旋转矢量的解决方案吗？我终于有了解决方案，就在这里。
Python，ROS几何库，numpy
我的实际代码/数学简称：
1）通过滚动和俯仰旋转激光器的位置和方向。
axes='sxyz'的意思是：静态轴，施加横摇，俯仰，偏航。 quaternion_matrix根据四元数创建一个4x4变换矩阵。laser = (1,1,1,0) # laser position
orientation = (1,0,0,0) # laser orientation
roll, pitch, _ = list(euler_from_quaternion(q, axes='sxyz'))
q = quaternion_from_euler(roll, pitch, 0, axes="sxyz")
laser = numpy.dot(quaternion_matrix(q), laser)
orientation = numpy.dot(quaternion_matrix(q), orientation)
2）代数解决方案：绕Z旋转并产生偏航
laser = [-sin(a)*laser[1] + cos(t)*laser[0],
cos(t)*laser[1] + sin(t)*laser[0],
laser[2]]
orientation = [-sin(a)*orientation[1] + cos(t)*orientation[0],
cos(t)*orientation[1] + sin(t)*orientation[0],
orientation[2]]
重要提示：自旋转以来如果不缩放向量，则K因子的分母是一个常数。然后，我们可以通过预先计算方向矢量的长度来简化它。
M = 100 # distance
K = sqrt(M^2 / (orientation[0]^2 + orientation[01]^2 + orientation[1]^2))
PointOnWall = [ K * orientation[0] + laser[0],
K * orientation[1] + laser[1],
K * orientation[2] + laser[2]]
4）代数解决方案：用两个激光在此上获得墙。
两个“ PointOnWall”方程应该提供足够的数据来获得偏航。知道这是（-1,0,0）法线，我可以从两点找到2个平面：
5）代数解决方案：测量YAW。
一架飞机在另一架飞机上（通过XMaxima），我们得到：
def getYaw(position1, orientation1, measure1, position2, orientation2, measure2):
length1 = length(orientation1)
length2 = length(orientation2)
k1 = measure1/length1
k2 = measure2/length2
numerator = -k2*orientation2[0] + k1*orientation1[0] + position1[0] - position2[0]
denominator = -k2*orientation2[1] + k1*orientation1[1] + position1[1] - position2[1]
return atan(numerator/denominator)
滚动和俯仰请勿干涉，因为位置和方向已预先旋转。
评论
$ \ begingroup $
这是您自己的问题的答案，还是该问题的更多详细信息？
$ \ endgroup $
–Ben♦
16年2月28日在0:05
$ \ begingroup $
不完整的答案：/主要是为了说明我现在的状况，也许是我做错了。
$ \ endgroup $
– Alexis Paques
16-2-28在1:10
$ \ begingroup $
我终于有了完整的解决方案：D
$ \ endgroup $
– Alexis Paques
16-2-28在14:53
评论
我们可以通过PixHawk的MavLink轻松获得方向。严重的是，由于在室内应用，围绕Z的偏航，航向，方位不稳定，而俯仰和横滚则稳定。我正在编辑问题以添加设置