平面几何数学基础

0. 概要

这篇文章讲解了在对机械臂进行运动学求解时所需的数学知识，大多都是高中数学基础，向量相关的属于大学线性代数的内容。希望你可以耐心看下去，这对你理解后面的算法有帮助。

如果对公式推导的详细过程有困难，可以暂时跳过，记住结论即可。

1. 三角函数

1.1 三角函数基础

1.2 正切与反正切

1.3 arctan2

上面的公式没有考虑 $a$ ， $b$ 之间的正负差异，例如上图，角 $\theta _1$ ，角 $\theta _2$ 他们实际的角度相差 $180^。$ ，但是他们的 $arctan$ 的取值都是相同的。

。

为了解决这个问题，我们需要引入另外一个函数 $arctan2$

这个函数分别传入 $d_x$ 和 $d_y$ ，注意他们是用逗号分隔的，函数对 $\theta$ 的取值范围约束如下。

。 。

所以

。

。

1.4 已知AB求夹角

如果已知 $A$ 点与 $B$ 点各自的坐标：

求 $AB$ 直线与 $x$ 轴的夹角

1.5 已知一点、直线和夹角，求另一点

如果 $A$ 点坐标已知， $A$ 点与 $B$ 点之间的线段长度已知 (计做 L )， $AB$ 所在的直线与 $x$ 轴的夹角已知，则 $B$ 点的坐标也可以推出。

1.6 余弦公式 - 已知三边求夹角

三边求夹角的Python实现代码

import math
def cal_tri_angle(a, b, c):
    '''返回边a与边b之间的夹角，弧度制'''
    try:
        theta = math.acos((a**2 + b**2 - c**2) / (2 * a * b))
    except ValueError as e:
        print(e)
        print('a={}, b={}, c={}'.format(a, b, c))
        exit(-1)
    return theta

2. 向量

向量叉乘主要用于辅助筛选候选的关节坐标，判断关节是朝外，还是朝里。

2.1 向量叉乘 (叉积)

我们不光要知道向量与向量之间的夹角是多少，而且想知道，从向量 $a$ 到向量 $b$ 到底是逆时针旋转，还是顺时针旋转。

向量 $a$ 与向量 $b$ 之间的叉乘用符号 $\times$ ， $||a||$ 与 $||b||$ 代表向量的模， $\theta$ 是两个向量之间的夹角。

。 。

向量 $a$ 与向量 $b$ 之间构成一个平面，向量 $n$ 是垂直于这个平面的单位向量。向量 $n$ 的方向，满足右手法则，伸出你的右手，从向量 $a$ 旋转到向量 $b$ ，大拇指所指向的方向就是向量 $n$ 的方向。

右手法则

如果向量 $a$ 与向量 $b$ 平行，即 $a$ 与 $b$ 之间的夹角为 $0^。$ 或 $180^。$ 。叉乘的结果为零向量。

向量叉乘

另外向量叉乘满足反交换率，叉乘的顺序先后不一样，得到的向量方向相反。

二维坐标系下叉乘的计算与几何含义

叉乘的几何意义是以两向量为邻边的 平行四边形的有向面积 。

如果

则说明，从向量 $a$ 旋转到向量 $b$ 是逆时针旋转。如果：

说明从向量 $a$ 旋转到向量 $b$ 是顺时针旋转。

Python代码片段如下：

def vector_cross_product(x1, y1, x2, y2):
    '''向量叉乘'''
    return x1 * y2 - x2 * y1

def is_vector_ccw(x1, y1, x2, y2):
    '''向量a是否是逆时针旋转到向量b'''
    return vector_cross_product(x1, y1, x2, y2) > 0

2.2 叉乘矩阵

将两个向量之间的叉乘运算改写为 叉乘矩阵 与向量的乘积。

定义运算符 $\hat{}$ ，它可以将一个向量转换为 反对称矩阵 的形式。

向量叉乘可以改写为矩阵与向量乘积的形式

所以叉乘又可以写为

3. 圆

通过计算得到圆与圆之间的交点，作为关节坐标，然后辅助上面介绍的叉乘，对关节坐标进行筛选。

3.1 圆的表达式

圆有两种表示：

圆的标准方程

其中 $(x_0, y_0)$ 是圆心， $r$ 是半径。

圆的参数方程

3.2 两个圆求交点

圆1用参数方程表示，圆2用标准方程表示。

为什么要这样做？这样减少展开项的复杂度

圆1

圆2

把圆1的方程带入圆2的方程

展开之后整理得到

令

公式 $(3.8)$ 就可以写成

且三角函数有如下属性

展开得到

令

公式 $(3.15)$ 简化为

这是关于 $cos(\theta)$ 的二元一次方程组

首先判断是否

如果大于0则说明两个圆之间有交点，此方程组可解，根据二元一次方程组的公式可得 $cos(\theta)$ 的两个解分别为：

其中 $cos(\theta)$ 解出来，判断这两个根是否满足：

接下来如果想继续求交点，就必须把 $\theta$ 求解出来，而一个 $cos(\theta)$ 的值，对应了两个可能的 $\theta$ 取值。

把 $\theta$ 带入圆1的参数化方程里面，这里拿 $\theta_1$ 举例：

得到交点

然后再判断 $A$ 点距离圆2圆心的距离是不是 $r_2$

判断 $dis == r2$ ，如果相等这个点就是两个圆之间的交点。

求解两个圆之间交点的代码 src/geometry_utils.py

import math

def circle_inter_posi(x1, y1, r1, x2, y2, r2):
    '''
    圆之间的交叉点
    '''
    distance = math.sqrt((x1 - x2)**2 + (y1 - y2)**2)

    if distance > (r1 + r2) or (x1 == x2 and y1 == y2):
        print('Error: circle has not intersect points')
        return False, None
    else:
        pass
        # print('distance: {}'.format(distance))
    a = 2 * r1 * (x1 - x2)
    b = 2 * r1 * (y1 - y2)
    c = (r2**2 - r1**2) - (x1 - x2)**2 - (y1 - y2)**2

    p = a**2 + b**2
    q = -2 * a * c
    r = c**2 - b**2

    cos_theta_1 = (-q + math.sqrt(q**2 - 4*p*r))/ (2 * p)
    cos_theta_2 = (-q - math.sqrt(q**2 - 4*p*r))/ (2 * p)

    # 存放所有可能的角度
    angle_list = []
    if abs(cos_theta_1) < 1:
        angle_list.append(math.acos(cos_theta_1))
    if abs(cos_theta_2) < 1:
        angle_list.append(math.acos(cos_theta_2))
	# 把角度负值也添加到angle_list中
    for i in range(len(angle_list)):
        theta = angle_list[i]
        if theta == 0.0:
            # 如果角度为0则不需要反转，因为-0跟0是同一个角度
            continue
        angle_list.append(-1*theta)
    
    angle_list = list(set(angle_list))
    # 存放交点
    result = []
    # 遍历验证角度是否正确
    for angle in angle_list:
        ix = x1 + r1 * math.cos(angle)
        iy = y1 + r1 * math.sin(angle)
        
        if abs(math.sqrt((ix-x2)**2 + (iy-y2)**2)-r2) <= 1e-3:
            result.append((ix, iy))
            
    return True, result