一个高中数学题

51st元素

已于 2022-11-02 20:22:53 修改

阅读量613

点赞数

分类专栏：视觉引导文章标签：几何学

于 2021-06-19 11:16:55 首次发布

本文链接：https://blog.csdn.net/qq_40564377/article/details/118015372

版权

视觉引导专栏收录该内容

2 篇文章 0 订阅

订阅专栏

一、一切从一个公式说起

假设在一个坐标系T中，存在一点A(x,y), 坐标系T旋转角度θ后，求点A在新坐标系T'下新坐标

A'(x', y');解法如下；参考：公式参考

新点的求解公式：

这里有个关键点：角度的方向，这里从X轴转向Y轴；

从Y轴转向X轴：

公式：

用一个公式表示：

θ ：从x轴转向y轴带入正值，从y轴转向x轴带入负值

二、一个高中数学题

已知一个目标A点在原来坐标中的坐标为(x1,y1);该坐标系旋转θ得到新的坐标系，A点在新坐标系下的坐标为（x2，y2），求旋转点的坐标；

2.1 先来分析下，这个问题成不成立

坐标是依据坐标系而来的，要求解的旋转点坐标是新坐标系下的坐标还是旧坐标系下的坐标？

其实对于旋转点而言，新坐标系下和旧的坐标系下的值是一样的。通俗的讲，绕哪个点，哪个点坐标不变。不信你拿着一张纸画一个坐标系，用钉子穿过（20，30）这个点，把纸钉在墙上，然后随便转动这张纸，钉子是不是一直在（20，30）这个点？

2.2 求解

这个问题是上面公式的延申，直接上图。

图中，红点为钉子，即旋转轴，五角星为墙上的一个点，该点不会随着坐标系转动，只会在坐标系中有投影坐标。

与上边不同的是，旋转点的坐标不是（0，0），而是变成了（20，30），求解目标由求点在新坐标系下的坐标变成了，求旋转点的坐标；

现在就是要往上边公式上靠，把旋转点变为（0，0），设旋转点坐标为(x0,y0)通过平移实现；

然后求解：

为了简单，可以把(x1,y1)设置为(0,0)，

化简得到：

当(x2,y2)=（-9.09 ，0.44），θ为6.01°时，(x0,y0)=(-0.3541,86.7992)

三、在视觉引导中的应用

import matplotlib.pyplot as plt
import numpy
import math
# 符号定义：旧坐标系（标定坐标系）       T1：点[x1,y1]所在坐标系
#         新坐标系（实际坐标系）       T2：点[x2, y2]所在坐标系
#         点[x1,y1]=[0,0]          描述特征在标定坐标系下的位置坐标,建立特征时，选择为[0,0](必须)
#         点[x2,y2]                描述特征在实际坐标系下的位置坐标
#         点[x2_L,y2_L]            [x1,y1]在实际坐标系下的映射坐标
#         点[x1_L,x1_L]            [x2,y2]在标定坐标系下的映射坐标
#         点[x0,y0]                旋转轴在标定坐标系下（实际坐标系下）的坐标
#         角度R                    标定坐标系绕点[x0,y0]旋转R度，得到实际坐标系，

# -----------------------------求[x0,y0]-----------------------------------------
# 求[x0,y0]点的坐标是最关键的，该点是旋转中心，是所有计算的基础
# 此处角度r和图像中特征旋转角度相反，图像中特征旋转-6.01,此时r = 6.01
x2,y2,r=[30,60,math.radians(6.01)]
x0 = x2/2 + y2*(math.cos(r)+1)/(2*math.sin(r))
y0 = y2/2 + x2*math.sin(r)/(2*(math.cos(r)-1))
print('旋转轴的坐标为：[x0,y0]=[{},{}]'.format(x0,y0))

# 计算两点到原点的距离,验证正确性
l1 = math.pow(-x0,2) + math.pow(-y0,2)
l2 = math.pow(x2-x0,2) + math.pow(y2-y0,2)
print("旋转轴到点[x1,y1]的距离l1:{},到点[x2,y2]的距离l2:{}".format(l1,l2))

# ------------------------------知道旋转后的点反求----------------------------------------
#  知道旋转点和实际坐标[x2,y2],反求[x1,y1]
r = math.radians(-6.01)
x1 = (x2-x0)*math.cos(r)+(y2-y0)*math.sin(r) + x0
y1 = (y2-y0)*math.cos(r)-(x2-x0)*math.sin(r) + y0
print("标定坐标系下[x1,y1]=[{},{}]".format(x1,y1))
print("---------------------------------------------")
# ---------------------------------引导应用-----------------------------------------------
# 需求： 工具坐标下的引导，要保证夹爪和目标之间的想对关系不变
#       不变的本质，是描述特征在相机视野中的坐标值为定值=[0,0]，
#       这就转化为，如何通过旋转、平移操作，让特征坐标值[x2,y2]=[0,0]
# 方法：有两种方案，先旋转后平移，先平移后旋转
# 假设实际坐标系下[x2, y2] = [70, 10]，实际坐标系与标定坐标系的夹角R=30°, 现在看如何变化能到 [x2,y2]=[0,0]
x2 = 70
y2 = 10
x1 = 0
y1 = 0
R = 30

# 方案一：先平移后旋转
# 通过旋转的角度，推出原始标定点（x1，y1）在新坐标系下的理论值（x2_L,y2_L）
# 求理论旋转后坐标
print("方案一:")
r = math.radians(R)
x2_L = (x1-x0)*math.cos(r)+(y1-y0)*math.sin(r) + x0
y2_L = (y1-y0)*math.cos(r)-(x1-x0)*math.sin(r) + y0
print("在实际坐标系中理论映射为：x2_L：{} y2_L：{}".format(x2_L, y2_L))
# 然后计算，新坐标系下（x2，y2），与理论点（x2_L,y2_L）的差值,
# 求得在新坐标下的平移偏移量
dtx1 = x2 - x2_L
dty1 = y2 - y2_L
print("在实际坐标下的偏差为：dtx1：{} dty1：{}".format(dtx1, dty1))
# 然后机器人，先走偏移量，后旋转-r，这种称为先平移后旋转；kuka机器人指令xp1：frame，走的就是先平移后旋转。
# 视觉发送坐标为，目的x2向x2_L上靠，机器人走完后，x2=x2_L
send_1 = [dtx1,dty1,-r]
print("然后机器人，先走偏移量，再旋转，这种称为先平移后旋转；视觉发送坐标为",send_1)
print("---------------------------------------------")


# 方案二：先旋转后平移
# 通过旋转角度，推出实际坐标系下的点（x2，y2）在标定坐标下的理论值（x1_L,y1_L）
# 注意的是，此时，角度带入-R
print("方案二:")
r = math.radians(-R)
x1_L = (x2-x0)*math.cos(r)+(y2-y0)*math.sin(r) + x0
y1_L = (y2-y0)*math.cos(r)-(x2-x0)*math.sin(r)  + y0
print("在标定坐标系下理论映射为：x1_L：{} y1_L：{}".format(x1_L, y1_L))
# 然后计算，标定坐标系下（x1，y1），与理论点（x1_L,y1_L）的差值,
# 求得在新坐标下的平移偏移量
dtx2 =  x1_L - x1
dty2 =  y1_L - y1
print("标定下的偏差为：dtx2：{} dty2：{}".format(dtx2, dty2))
# 然后机器人，先旋转r，再走偏移量，这种称为先旋转后平移；
# 视觉发送坐标为，目的x1_L向x1上靠，机器人走完后，x1_L=x1
send_2 = [dtx2,dty2,r]
print("然后机器人，先旋转r，再走偏移量，这种称为先旋转后平移；视觉发送坐标为",send_2)
print("---------------------------------------------")

# # 两种标定的转化关系
print("先平移后旋转------→先旋转后平移")
r = math.radians(-R)
tempx = send_1[0]*math.cos(r) + send_1[1]*math.sin(r)
tempy = send_1[1]*math.cos(r) - send_1[0]*math.sin(r)
print("tempx:",tempx,"tempy:",tempy)