Python 机器人学 —— 天舟2号在轨转位对接动画

荷碧TongZJ

已于 2023-04-09 21:53:27 修改

阅读量2.3k

点赞数 8

分类专栏：课程设计 matplotlib 绘图文章标签： python 矩阵

于 2022-03-21 23:39:26 首次发布

本文链接：https://blog.csdn.net/qq_55745968/article/details/123649127

版权

课程设计同时被 2 个专栏收录

6 篇文章 5 订阅

订阅专栏

matplotlib 绘图

5 篇文章 0 订阅

订阅专栏

记参考坐标系U，轨道坐标系P，天宫坐标系G，天舟坐标系R

这两个坐标系是固定的，利用右乘齐次变换矩阵以实现：天宫相对于轨道的变换、天舟相对于天宫的变换

θ：天宫x轴和轨道x轴的夹角
ρ：天舟y轴与天宫y轴的夹角
φ：天宫z轴和轨道z轴的夹角
r：轨道半径

对于天宫坐标系有：

$G=P \times Rot_{a}(\theta) \times Trans(-r, 0, 0) \times Rot_{o}(\phi)$

即轨道坐标系 -> 绕a轴旋转 -> 沿n轴平移 -> 绕o轴旋转 -> 天宫坐标系

对于天舟坐标系有：

$R=G \times Rot_{n}(\rho)$

即天宫坐标系 -> 绕n轴旋转 -> 天舟坐标系

在绘制天宫和天舟的过程中，再添加相对于天宫坐标系o轴的平移变换，调整相对的位置关系即可

程序流程图如下：

环境设置

这次实验主要用到了 numpy、matplotlib，另外还用到了我自己编写的一个模块 coord.py

coord.py 中的 CoordSys_3d 用于描述齐次坐标系，并为图形的仿射变换提供了接口，源代码位于：https://hebitzj.blog.csdn.net/article/details/129912954

from functools import partial

import matplotlib.pyplot as plt
import numpy as np

# coord.py 详见: https://blog.csdn.net/qq_55745968/article/details/129912954
from coord import CoordSys_3d

rot = CoordSys_3d.rot
trans = CoordSys_3d.trans

red = 'orangered'
orange = 'orange'
yellow = 'yellow'
green = 'greenyellow'
cyan = 'aqua'
blue = 'deepskyblue'
purple = 'mediumpurple'
pink = 'violet'

FIG_LIMIT = 15  # 3D 工作站边界
PATHWAY_R = 12  # 轨道半径
TIANGONG_R = 1
TIANGONG_H = 2.5  # 天宫每个空间站的尺寸
PATHWAY = CoordSys_3d().rela_tf(rot(30, 'y'))  # 轨道坐标系
PACE_NUM = 480  # 每一圈的步数
PACE_LENGTH = 360 / PACE_NUM  # 每一步的角度
TIME_DELAY = 0.001  # pause 时间

绘图函数

优化画面流畅度有几个手段：

改变 array 的数据类型：float32 -> float16
减小圆等效多边形的边数：这样改进效果有限，毕竟边数过少时会严重降低颜值
将函数相关数据缓存：采用 lambda 关键字对绘制命令进行封装，通过数据的复用减少计算

这几个绘图函数均利用了 Coord_3d 的 apply 函数，以便对图形进行仿射变换

ROUND_EDGE = 30  # 圆等效多边形边数
DTYPE = np.float16  # 矩阵使用的数据类型


def figure3d():
    ''' 创建3d工作站'''
    figure = plt.subplot(projection='3d')
    tuple(getattr(figure, f'set_{i}label')(i) for i in 'xyz')
    return figure


def cylinder(figure, state: CoordSys_3d,
             R: float, h: float, r: float = 0,
             axis: int = 2, smooth: int = 2):
    ''' 以 state 的 z 轴为主轴绘制圆柱
        figure: 3D 工作站对象
        state: CoordSys_3d 齐次坐标系
        R: 圆柱底面外径
        r: 圆柱底面内径
        h: 圆柱高度
        axis: 圆柱两底面圆心连线所在的轴索引
        smooth: 图像细致程度 (至少 2)'''
    # 当主轴为 x,y 时, 对坐标系进行变换
    if axis < 2:
        rotate = CoordSys_3d.rot(90, 'y' if axis == 0 else 'x')
        state = state.rela_tf(rotate)
    func = []
    theta = np.linspace(0, 2 * np.pi, ROUND_EDGE, dtype=DTYPE)
    z = np.linspace(-h / 2, h / 2, smooth, dtype=DTYPE)
    theta, z = np.meshgrid(theta, z)
    # 绘制圆柱内外曲面: 以 z 轴为主轴, 原点为中心
    x, y = np.cos(theta), np.sin(theta)
    func.append(partial(figure.plot_surface, *state.apply(x * R, y * R, z)))
    func.append(partial(figure.plot_surface, *state.apply(x * r, y * r, z)))

    phi = np.linspace(0, 2 * np.pi, ROUND_EDGE, dtype=DTYPE)
    radius = np.linspace(r, R, 2, dtype=DTYPE)
    phi, radius = np.meshgrid(phi, radius)
    # 绘制上下两底面: 法向量为 z 轴, 原点为中心, 在 z 轴上偏移得到两底面
    x, y = np.cos(phi) * radius, np.sin(phi) * radius
    z = np.zeros_like(x)
    for dz in (-h / 2, h / 2):
        s = state.rela_tf(CoordSys_3d.trans(dz=dz))
        func.append(partial(figure.plot_surface, *s.apply(x, y, z)))
    # 返回函数流的执行函数
    return lambda cmap: tuple(f(cmap=cmap) for f in func)


def ball(figure, state: CoordSys_3d, r: float):
    ''' 绘制球体
        figure: 3D 工作站对象
        state: CoordSys_3d 齐次坐标系
        r: 球体半径'''
    theta = np.linspace(0, 2 * np.pi, ROUND_EDGE, dtype=DTYPE)
    phi = np.linspace(0, np.pi, ROUND_EDGE // 2, dtype=DTYPE)
    theta, phi = np.meshgrid(theta, phi)
    sin_phi = np.sin(phi) * r
    x, y, z = state.apply(x=np.cos(theta) * sin_phi, y=np.sin(theta) * sin_phi, z=np.cos(phi) * r)
    return lambda cmap: figure.plot_surface(x, y, z, cmap=cmap)


def circle(figure, state, r, linestyle='-'):
    ''' 绘制圆环
        figure: 3D 工作站对象
        state: 描述齐次坐标系
        r: 圆环半径
        return: 可缓存函数'''
    theta = np.linspace(0, 2 * np.pi, ROUND_EDGE, dtype=DTYPE)
    x, y, z = state.apply(np.cos(theta) * r, np.sin(theta) * r, np.zeros_like(theta))
    return lambda c: figure.plot(x, y, z, c=c, linestyle=linestyle)

天宫绘制

def airship(state, interval, cmap='summer'):
    ''' 绘制天宫的两个工作站
        state: 处在轨道上的坐标系
        interval: 天宫两空间站的间隔
        return: 与天舟 2 号对接的空间站坐标系'''
    dist_unit = .5 * (TIANGONG_H + interval)
    head = state.rela_tf(trans(0, dist_unit, 0))
    tail = state.rela_tf(trans(0, - dist_unit, 0))
    for s in (head, tail):
        cylinder(fig, s, TIANGONG_R, TIANGONG_H, r=TIANGONG_R / 2, axis=1)(cmap)
    return head

任务管理器

class Manager:
    interval = 0.8
    tran = trans(-PATHWAY_R, 0, 0)

    def __init__(self):
        ''' 任务管理器'''
        self.tiangong_theta = 0
        self.tianzhou_rho = 0
        self.tiangong_phi = 0
        # 三个角度常量
        self.cmap_list = ['tab20c', 'tab20b']
        self.earth = ball(fig, CoordSys_3d(), 5)
        self.pathway = circle(fig, PATHWAY, PATHWAY_R, linestyle='--')
        # 地球、轨道绘制函数

    def show(self):
        ''' 绘制天宫、天舟'''
        self.refresh()
        # 轨道坐标系 -> 绕 z 轴旋转 -> 沿 x 轴平移 -> 天宫坐标系
        tiangong = PATHWAY.rela_tf(rot(self.tiangong_theta, 'z')
                                   ).rela_tf(self.tran
                                             ).rela_tf(rot(self.tiangong_phi, 'y'))
        # 绘制天宫坐标系
        tiangong = airship(tiangong, self.interval)
        tiangong.plot_coord_sys(length=6, linewidth=3)
        # 天宫坐标系 -> 绕 x 轴旋转 -> 沿 y 轴平移 -> 天舟坐标系
        tianzhou = tiangong.rela_tf(rot(self.tianzhou_rho, 'x')
                                    ).rela_tf(trans(0, self.interval + TIANGONG_H, 0))
        cylinder(fig, tianzhou, TIANGONG_R, TIANGONG_H, r=TIANGONG_R / 2, axis=1)('Wistia')
        plt.pause(TIME_DELAY)

    def move(self):
        ''' 绕轨道运动一步'''
        self.tiangong_theta -= PACE_LENGTH

    def refresh(self):
        ''' 画面刷新'''
        fig.cla()
        fig.view_init(azim=self.tiangong_theta + 70)
        # 设置工作站边界
        for set_lim in [fig.set_xlim3d, fig.set_ylim3d, fig.set_zlim3d]:
            set_lim(-FIG_LIMIT, FIG_LIMIT)
        plt.tight_layout()
        self.earth(self.cmap_list[int((-self.tiangong_theta // 20) % 2)])
        self.pathway(pink)

主循环

# 初始化3d工作站
fig = figure3d()
manager = Manager()
manager.show()

# 前两周的运动
pace_num = PACE_NUM * 2
for pace in range(pace_num):
    manager.move()
    manager.tianzhou_rho += 90 / pace_num
    manager.show()

# 第三周的运动
for pace in range(PACE_NUM):
    manager.move()
    manager.tiangong_phi += 90 / PACE_NUM
    manager.show()

# 持续运动
while 1:
    manager.move()
    manager.show()