CN111977006A - Method and device for initializing joint angle and aircraft - Google Patents

Method and device for initializing joint angle and aircraft Download PDF

Info

Publication number
CN111977006A
CN111977006A CN202010801302.4A CN202010801302A CN111977006A CN 111977006 A CN111977006 A CN 111977006A CN 202010801302 A CN202010801302 A CN 202010801302A CN 111977006 A CN111977006 A CN 111977006A
Authority
CN
China
Prior art keywords
motor
joint angle
determining
attitude quaternion
base end
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202010801302.4A
Other languages
Chinese (zh)
Inventor
汪康利
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Autel Robotics Co Ltd
Shenzhen Autel Intelligent Aviation Technology Co Ltd
Original Assignee
Autel Robotics Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Autel Robotics Co Ltd filed Critical Autel Robotics Co Ltd
Priority to CN202010801302.4A priority Critical patent/CN111977006A/en
Publication of CN111977006A publication Critical patent/CN111977006A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B64—AIRCRAFT; AVIATION; COSMONAUTICS
    • B64D—EQUIPMENT FOR FITTING IN OR TO AIRCRAFT; FLIGHT SUITS; PARACHUTES; ARRANGEMENTS OR MOUNTING OF POWER PLANTS OR PROPULSION TRANSMISSIONS IN AIRCRAFT
    • B64D47/00—Equipment not otherwise provided for
    • B64D47/08—Arrangements of cameras
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B64—AIRCRAFT; AVIATION; COSMONAUTICS
    • B64C—AEROPLANES; HELICOPTERS
    • B64C39/00—Aircraft not otherwise provided for
    • B64C39/02—Aircraft not otherwise provided for characterised by special use
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B64—AIRCRAFT; AVIATION; COSMONAUTICS
    • B64U—UNMANNED AERIAL VEHICLES [UAV]; EQUIPMENT THEREFOR
    • B64U2101/00—UAVs specially adapted for particular uses or applications
    • B64U2101/30—UAVs specially adapted for particular uses or applications for imaging, photography or videography

Abstract

The invention relates to the technical field of aircrafts, and discloses a method and a device for initializing a joint angle and an aircraft, wherein the method comprises the following steps: acquiring a first joint angle corresponding to the electrifying time of the first motor, a second electrical angle corresponding to the electrifying time of the second motor and a third electrical angle corresponding to the electrifying time of the third motor, further determining a joint angle combination, determining an attitude quaternion corresponding to the joint angle combination and an attitude quaternion of the base end, and determining an attitude quaternion of the tool end according to the attitude quaternion of the base end and the attitude quaternion of the joint angle combination; and determining the initialized joint angle combination of the three-axis motor according to the attitude quaternion of the tool end and the reading of the three-axis accelerometer of the tool end. By carrying out self-checking on the first motor and determining the initialized joint angle combination of the three-axis motor, the invention can improve the self-checking efficiency of the three-axis pan-tilt after being electrified.

Description

Method and device for initializing joint angle and aircraft
Technical Field
The invention relates to the technical field of aircrafts, in particular to a method and a device for initializing a joint angle and an aircraft.
Background
Aircraft, such as Unmanned Aerial Vehicle (UAV), also called as Unmanned Aerial Vehicle, has been increasingly widely used due to its advantages of small size, light weight, maneuverability, quickness in response, Unmanned driving, low operation requirements, and the like. The unmanned aerial vehicle is generally provided with a holder, such as a three-axis holder, the three-axis holder comprises a three-axis motor, and a linear hall sensor is generally arranged on the three-axis motor and used for detecting a magnetic field of a motor rotor and further calculating an electrical angle of the motor, so that a joint angle is determined, and control of the motor is further realized.
At present, in order to obtain the joint angle of the motor, the motor can carry out self-checking after being electrified, so that the normal work of the holder is realized, but the mode needs to carry out self-checking on the three-axis motor after being electrified, so that the working efficiency is influenced.
Disclosure of Invention
The embodiment of the invention provides a joint angle initialization method, a joint angle initialization device and an aircraft, solves the problem of low efficiency caused by the fact that three-axis motors of the existing aircraft all need self-checking, and improves the self-checking efficiency of a three-axis tripod head after being electrified.
In order to solve the above technical problems, embodiments of the present invention provide the following technical solutions:
in a first aspect, an embodiment of the present invention provides an initialization method for a joint angle, which is applied to an aircraft, where the aircraft is provided with a three-axis pan-tilt, the three-axis pan-tilt includes three-axis motors, which are a first motor, a second motor, and a third motor, respectively, the second motor corresponds to a second pole pair number, the third motor corresponds to a third pole pair number, the three-axis pan-tilt further includes a base end and a tool end, the tool end is provided with a three-axis accelerometer, and the method includes:
acquiring a first joint angle corresponding to the power-on time of the first motor, a second electrical angle corresponding to the power-on time of the second motor and a third electrical angle corresponding to the power-on time of a third motor;
traversing second joint angles and third joint angles of the second motor and the third motor according to a second electrical angle and a second pole pair number of the second motor and a third electrical angle and a third pole pair number of the third motor, and determining joint angle combinations of the first motor, the second motor and the third motor;
determining an attitude quaternion corresponding to the joint angle combination and an attitude quaternion of the base end, and determining an attitude quaternion of the tool end according to the attitude quaternion of the base end and the attitude quaternion of the joint angle combination;
and determining the initialized joint angle combination of the three-axis motor by combining the readings of the three-axis accelerometer of the tool end according to the attitude quaternion of the tool end.
In some embodiments, the base end is provided with a three-axis accelerometer, and the determining the attitude quaternion of the base end comprises:
acquiring the reading of the triaxial accelerometer at the base end, fixing the yaw angle of the base end, and calculating the pitch angle and the roll angle of the base end according to the reading of the triaxial accelerometer at the base end;
and determining the attitude quaternion of the base end according to the yaw angle, the pitch angle and the roll angle of the base end.
In some embodiments, after determining the joint angle combination of the first, second, and third motors, the method further comprises: screening joint angle combinations of the first motor, the second motor and the third motor, determining the screened joint angle combinations, and determining attitude quaternions corresponding to the joint angle combinations, wherein the attitude quaternions comprise:
and determining the posture quaternion corresponding to each screened joint angle combination.
In some embodiments, the obtaining a first joint angle corresponding to the first motor at the power-on time includes:
and determining a first joint angle corresponding to the first motor in a self-checking collision limiting mode.
In some embodiments, the method further comprises:
calculating an accelerometer module value of the base end and an accelerometer module value of the tool end;
and determining whether the cradle head is in a static state or not according to the accelerometer module value of the base end and the accelerometer module value of the tool end.
In some embodiments, the determining the attitude quaternion of the tool end from the attitude quaternion of the base end and the attitude quaternion of the joint angle combination includes:
and carrying out quaternion multiplication on the attitude quaternion of the base end and the attitude quaternion of the joint angle combination to determine the attitude quaternion of the tool end.
In some embodiments, the determining an initialized joint angle combination of the three-axis motors from the attitude quaternion of the tool end in combination with the readings of the three-axis accelerometer of the tool end comprises:
determining a rotation matrix according to the attitude quaternion of the tool end;
generating a gravity vector according to the reading of the triaxial accelerometer of the tool end;
and determining the initialized joint angle combination of the three-axis motor according to the rotation matrix and the gravity vector.
In some embodiments, said determining an initialized joint angle combination for said three-axis motor from said rotation matrix and said gravity vector comprises:
acquiring a column vector of the rotation matrix;
performing dot product operation on the column vector of the rotation matrix and the gravity vector to generate a plurality of dot product results;
and taking the first joint angle, the second joint angle and the third joint angle corresponding to the maximum value of the point multiplication result as the initialized joint angle combination of the three-axis motor.
In a second aspect, an embodiment of the present invention provides an initialization apparatus for a joint angle, which is applied to an aircraft, where the aircraft is provided with a three-axis pan-tilt, the three-axis pan-tilt includes three-axis motors, respectively a first motor, a second motor, and a third motor, the second motor corresponds to a second pole pair number, the third motor corresponds to a third pole pair number, the three-axis pan-tilt further includes a base end and a tool end, the tool end is provided with a three-axis accelerometer, and the apparatus includes:
the acquisition unit is used for acquiring a first joint angle corresponding to the electrifying time of the first motor, a second electrical angle corresponding to the electrifying time of the second motor and a third electrical angle corresponding to the electrifying time of the third motor;
the joint angle combination unit is used for traversing second joint angles and third joint angles of the second motor and the third motor according to a second electric angle and a second pole logarithm of the second motor and a third electric angle and a third pole logarithm of the third motor, and determining joint angle combinations of the first motor, the second motor and the third motor;
the attitude quaternion unit is used for determining an attitude quaternion corresponding to the joint angle combination and an attitude quaternion of the base end, and determining an attitude quaternion of the tool end according to the attitude quaternion of the base end and the attitude quaternion of the joint angle combination;
and the initialized joint angle combination unit is used for determining the initialized joint angle combination of the three-axis motor according to the attitude quaternion of the tool end and by combining the reading of the three-axis accelerometer of the tool end.
In some embodiments, the attitude quaternion unit is specifically configured to:
acquiring the reading of the triaxial accelerometer at the base end, fixing the yaw angle of the base end, and calculating the pitch angle and the roll angle of the base end according to the reading of the triaxial accelerometer at the base end;
and determining the attitude quaternion of the base end according to the yaw angle, the pitch angle and the roll angle of the base end.
In some embodiments, the apparatus further comprises:
and the screening unit is used for screening joint angle combinations of the first motor, the second motor and the third motor, determining the screened joint angle combinations, and determining the posture quaternion corresponding to the joint angle combinations.
In some embodiments, the obtaining unit is specifically configured to:
and determining a first joint angle corresponding to the first motor in a self-checking collision limiting mode.
In some embodiments, the apparatus further comprises:
the static state unit is used for calculating an accelerometer module value of the base end and an accelerometer module value of the tool end;
and determining whether the cradle head is in a static state or not according to the accelerometer module value of the base end and the accelerometer module value of the tool end.
In some embodiments, the attitude quaternion unit is specifically configured to:
and carrying out quaternion multiplication on the attitude quaternion of the base end and the attitude quaternion of the joint angle combination to determine the attitude quaternion of the tool end.
In some embodiments, the initialization joint angle combination unit is specifically configured to:
acquiring a column vector of the rotation matrix;
performing dot product operation on the column vector of the rotation matrix and the gravity vector to generate a plurality of dot product results;
and taking the first joint angle, the second joint angle and the third joint angle corresponding to the maximum value of the point multiplication result as the initialized joint angle combination of the three-axis motor.
In a third aspect, an embodiment of the present invention provides an aircraft, including:
a body;
the machine arm is connected with the machine body;
the power device is arranged on the fuselage and/or the horn and is used for providing flying power for the aircraft;
the three-axis holder is arranged on the machine body and comprises a three-axis motor, a first motor, a second motor and a third motor, and the three-axis holder further comprises a base end and a tool end;
the flight controller is arranged on the machine body;
wherein the flight controller includes:
at least one processor; and the number of the first and second groups,
a memory communicatively coupled to the at least one processor; wherein the content of the first and second substances,
the memory stores instructions executable by the at least one processor to enable the at least one processor to perform the method of initializing a joint angle as described above.
In a fourth aspect, embodiments of the present invention also provide a non-transitory computer-readable storage medium storing computer-executable instructions for enabling an aircraft to perform the method for initializing a joint angle as described above.
The invention provides an initialization method of a joint angle, which is applied to an aircraft, wherein the aircraft is provided with a three-axis pan-tilt, the three-axis pan-tilt comprises three-axis motors which are respectively a first motor, a second motor and a third motor, the second motor corresponds to a second pole logarithm, the third motor corresponds to a third pole logarithm, the three-axis pan-tilt also comprises a base end and a tool end, the tool end is provided with a three-axis accelerometer, and the method comprises the following steps: acquiring a first joint angle corresponding to the power-on time of the first motor, a second electrical angle corresponding to the power-on time of the second motor and a third electrical angle corresponding to the power-on time of a third motor; traversing second joint angles and third joint angles of the second motor and the third motor according to a second electrical angle and a second pole pair number of the second motor and a third electrical angle and a third pole pair number of the third motor, and determining joint angle combinations of the first motor, the second motor and the third motor; determining an attitude quaternion corresponding to the joint angle combination and an attitude quaternion of the base end, and determining an attitude quaternion of the tool end according to the attitude quaternion of the base end and the attitude quaternion of the joint angle combination; and determining the initialized joint angle combination of the three-axis motor by combining the readings of the three-axis accelerometer of the tool end according to the attitude quaternion of the tool end. By carrying out self-checking on the first motor and determining the initialized joint angle combination of the three-axis motor, the invention can improve the self-checking efficiency of the three-axis pan-tilt after being electrified.
Drawings
One or more embodiments are illustrated by way of example in the accompanying drawings, which correspond to the figures in which like reference numerals refer to similar elements and which are not to scale unless otherwise specified.
FIG. 1 is a detailed block diagram of an aircraft provided by an embodiment of the present invention;
fig. 2 is a schematic view of a three-axis pan-tilt provided by an embodiment of the present invention;
FIG. 3 is a flowchart illustrating a method for initializing a joint angle according to an embodiment of the present invention;
FIG. 4 is a detailed flowchart of step S30 in FIG. 3;
FIG. 5 is a detailed flowchart of step S40 in FIG. 3;
FIG. 6 is a detailed flowchart of step S43 in FIG. 5;
fig. 7 is a schematic structural diagram of an apparatus for initializing a joint angle according to an embodiment of the present invention;
FIG. 8 is a schematic diagram of a hardware configuration of an aircraft according to an embodiment of the present invention;
FIG. 9 is a connection block diagram of an aircraft provided by an embodiment of the present invention;
FIG. 10 is a schematic illustration of the powertrain of FIG. 9.
Detailed Description
In order to make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are some, but not all, embodiments of the present invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
In addition, the technical features involved in the embodiments of the present invention described below may be combined with each other as long as they do not conflict with each other.
The method for initializing the joint angle provided by the embodiment of the invention can be applied to various movable objects driven by motors or motors, including but not limited to aircrafts, robots and the like. Wherein the aircraft may include Unmanned Aerial Vehicles (UAVs), unmanned airships, and the like.
The joint angle initialization method is applied to a flight controller of an aircraft.
Referring to fig. 1, fig. 1 is a detailed structural diagram of an aircraft according to an embodiment of the present invention;
as shown in fig. 1, the aircraft 10 includes: the aircraft comprises a fuselage 11, a horn 12 connected with the fuselage 11, a power device 13 arranged on the horn 12, a cradle head 14 connected to the bottom of the fuselage 11, a camera 15 arranged on the cradle head 14 and a flight controller (not shown) arranged in the fuselage 11.
The flight controller is connected with a power device 13, and the power device 13 is installed on the aircraft body 11 and used for providing flight power for the aircraft 10. Specifically, the flight controller is configured to execute the above-mentioned method for initializing the joint angle to generate a control command, and send the control command to the electric governor of the power device 13, and the electric governor controls the driving motor of the power device 13 through the control command. Alternatively, the flight controller is configured to execute an initialization method of a joint angle so as to generate a control command and control the drive motor of the power unit 13 by the control command.
The body 11 includes: the robot arm assembly comprises a central shell and one or more arms connected with the central shell, wherein the one or more arms radially extend out of the central shell. The connection of the horn to the center housing may be an integral connection or a fixed connection. The power device is arranged on the machine arm.
The flight controller is used for executing the initialization method of the joint angle to determine the joint angle of the three-axis motor, conveniently generating a control command, and sending the control command to the electric regulator of the power device so that the electric regulator can control the driving motor of the power device through the control command. The controller is a device with certain logic processing capability, such as a control chip, a single chip, a Micro Control Unit (MCU), and the like.
The power unit 13 includes: the electric regulator drives a motor and a propeller. The electric speed regulator is positioned in a cavity formed by the mechanical arm or the central shell. The electric regulator is respectively connected with the controller and the driving motor. Specifically, the electric regulator is electrically connected with the driving motor and used for controlling the driving motor. The driving motor is arranged on the machine arm, and a rotating shaft of the driving motor is connected with the propeller. The propeller generates a force that causes the aircraft 10 to move, for example, a lift force or a thrust force that causes the aircraft 10 to move, under the drive of the drive motor.
The aircraft 10 accomplishes each of the prescribed speeds, motions (or attitudes) by electrically adjusting and controlling the driving motors. The electrically-controlled full-scale electronic speed regulator regulates the rotating speed of a driving motor of the aircraft 10 according to a control signal. The controller is an execution main body for executing the initialization method of the joint angle, and the controller generates a control instruction by electric regulation to control the driving motor. The principle of electrically adjusting and controlling a driving motor is roughly as follows: the drive motor is an open-loop control element that converts an electrical pulse signal into an angular or linear displacement. In the non-overload condition, the rotation speed and the stop position of the driving motor only depend on the frequency and the pulse number of the pulse signal and are not influenced by the load change, when the driver receives a pulse signal, the driver drives the driving motor of the power device to rotate by a fixed angle in a set direction, and the rotation of the driving motor runs by the fixed angle. Therefore, the electric regulation can control the angular displacement by controlling the number of the pulses, thereby achieving the purpose of accurate positioning; meanwhile, the rotating speed and the rotating acceleration of the driving motor can be controlled by controlling the pulse frequency, so that the purpose of speed regulation is achieved.
The main functions of the present aircraft 10 are aerial photography, real-time image transmission, high-risk area detection, etc. In order to realize functions of aerial photography, real-time image transmission, high-risk area detection and the like, the aircraft 10 is connected with a camera component. Specifically, the aircraft 10 and camera assembly are connected by a connecting structure, such as a vibration dampening ball or the like. The camera assembly is used for acquiring a shooting picture in the process of aerial photography of the aircraft 10.
Specifically, the camera module includes: cloud platform and shooting device. The head is connected to the aircraft 10. The shooting device is mounted on the cradle head, and the shooting device can be an image acquisition device and is used for acquiring images, and the shooting device includes but is not limited to: cameras, video cameras, scanners, camera phones, and the like. The cradle head is used for carrying the shooting device, so as to fix the shooting device, or freely adjust the posture of the shooting device (for example, change the height, the inclination angle and/or the direction of the shooting device) and stably maintain the shooting device at the set posture. For example, when the aircraft 10 performs aerial photography, the pan/tilt head is mainly used to keep the shooting device stably at a set posture, prevent the shooting device from shaking the shot image, and ensure the stability of the shot image.
The pan-tilt 14 is connected with the flight controller to realize data interaction between the pan-tilt 14 and the flight controller. For example, the flight controller sends a yaw command to the pan/tilt head 14, the pan/tilt head 14 obtains a speed and direction command of the yaw and executes the command, and data information generated after the yaw command is executed is sent to the flight controller, so that the flight controller detects the current yaw condition.
The cloud platform includes: cloud platform motor and cloud platform base. Wherein, the cloud platform motor is installed in cloud platform base. The flight controller also can control the pan tilt motor through the electricity of power device 13, and is concrete, and the flight controller is connected with the electricity accent, and the electricity accent is connected with pan tilt motor electricity, and the flight controller generates pan tilt motor control command, and the electricity accent is through pan tilt motor control command in order to control the pan tilt motor.
The holder base is connected with the body of the aircraft and is used for fixedly installing the camera shooting assembly on the body of the aircraft.
The holder motor is respectively connected with the holder base and the shooting device. This cloud platform can be for the multiaxis cloud platform, with it adaptation, the cloud platform motor is a plurality of, also every axle is provided with a cloud platform motor. The pan-tilt motor can drive the shooting device to rotate on one hand, so that the horizontal rotation and the pitching angle of the shooting rotating shaft can be adjusted, and the pan-tilt motor is manually and remotely controlled to rotate or automatically rotates by utilizing a program, so that the function of omnibearing scanning monitoring is achieved; on the other hand, in the process of aerial photography of the aircraft, the disturbance that the shooting device received is offset in real time through the rotation of cloud platform motor, prevents to shoot the device shake, guarantees the stability of shooting the picture.
In the invention, the holder is a three-axis holder, the holder motor is a three-axis motor, and the three-axis motor respectively comprises a first motor, a second motor and a third motor.
Specifically, the three-axis pan-tilt can be a stability augmentation device and a support device for mounting and fixing loads such as a camera or a sensor. In the embodiment of the invention, the three-axis pan-tilt comprises a yaw axis arm, a roll axis arm and a pitch axis arm, wherein each axis arm corresponds to a motor, namely a yaw axis motor (a first motor) for controlling the yaw axis arm to rotate, a roll axis motor (a second motor) for controlling the roll axis arm to rotate and a pitch axis motor (a third motor) for controlling the pitch axis arm to rotate, and the yaw axis arm, the roll axis arm and the pitch axis arm are correspondingly controlled to rotate through the yaw axis motor, the roll axis motor and the pitch axis motor, so that the attitude of the three-axis pan-tilt is controlled.
The three-axis pan-tilt comprises a tool end, wherein the tool end can be a shooting device, the shooting device is carried on the pan-tilt, an Inertial Measurement Unit (IMU) is arranged on the shooting device, and the IMU is a device for measuring the three-axis attitude angle (or angular velocity) and the acceleration of an object. Generally, a three-axis gyroscope and three-direction accelerometers are mounted in an IMU, that is, the angular velocity and acceleration of an object in a three-dimensional space are measured by the three-axis gyroscope and the three-axis accelerometers, and the attitude of the object is calculated by the measurement. To increase reliability, more sensors may be provided for each axis. Generally, the IMU is to be mounted at the center of gravity of the aircraft.
In the working process of the three-axis holder, a gyroscope signal at the tail end needs to be converted into a three-axis joint angular velocity through a Jacobian inverse matrix, and a three-axis joint angle needs to be used for calculating the Jacobian inverse matrix.
At present, most of small holders are provided with linear hall sensors on a three-axis brushless motor to detect the magnetic field of a motor rotor, and further calculate the electrical angle of the motor, so that the control of the motor is realized.
The electric angle and the joint angle of the motor do not correspond to each other and are related to the pole pair number of the motor. For an N-pair pole motor, N electrical angle cycles are contained within one joint angle cycle. Therefore, in order to obtain the joint angle of the motor, the motor is subjected to self-checking after being electrified, the motor is made to touch the mechanical limit of each shaft, then the joint angle of the motor is calculated through the electrical angle by taking the mechanical limit as a reference point, and therefore the normal work of the holder is achieved. The method has the advantages that the self-checking is carried out on the three-axis motor after the power-on, the efficiency is low, and the body feeling brought to a user is poor.
Based on the above problems, embodiments of the present invention provide an initialization method and apparatus for a joint angle, and an aircraft, so as to improve self-checking efficiency of a three-axis pan-tilt after being powered on.
The embodiments of the present invention will be further explained with reference to the drawings.
Example one
Referring to fig. 2, fig. 2 is a schematic view of a three-axis pan-tilt according to an embodiment of the present invention;
as shown in FIG. 2, the three-axis pan/tilt head includes three-axis motors, which are respectively a first motor M1A second motor M2And a third motor M3Wherein, first motor M is used for controlling the driftage axle arm of triaxial cloud platform and rotates, the second motor is used for controlling the roll axle arm of triaxial cloud platform and rotates, the third motor is used for controlling the pitch axle arm of triaxial cloud platform and rotates, and then realizes the control of triaxial cloud platform gesture, wherein, first motor M1Corresponding to the base end, the second motor M2Corresponding to the tool end, the third motor M3Corresponding to the end, i.e. the first motor is located at the position of the base end, the second motor is located at the position of the tool end, the third motor is located at the position of the end, the end is the end of the tool end, i.e. the tool end, wherein each motor corresponds to a pole pair number, assuming that the first motor M corresponds to a pole pair number1The corresponding first pole pair number is N1The second motor M2Corresponding second logarithmic logarithm of pole N2Said third motor M3The corresponding logarithm of the third pole is N3Wherein the electrical angle of the first motor at the time of electrifying is thetae1Electric of the second electric machineAngle thetae2The electrical angle of the third motor is thetae3The first joint angle corresponding to the first motor is thetaj1The second joint angle corresponding to the second motor is thetaj2The third joint angle corresponding to the third motor is thetaj3Wherein, the corresponding relation between the joint angle and the electrical angle is respectively as the following formula (1):
Figure BDA0002627483060000091
wherein the wrap () function functions to limit the joint angle between [ -pi, pi ].
Due to the first motor M1Is parallel to the Z-axis direction of the base end, so that when the base end is horizontal, the first motor M is1The rotation of (a) does not affect the value of gravity acting on the three-axis accelerometer at the tool end, so that the second motor M can be calculated by the base end accelerometer and the tool end accelerometer2And a third motor M3The joint angle of (a).
Specifically, please refer to fig. 3 again, fig. 3 is a schematic flow chart of an initialization method of a joint angle according to an embodiment of the present invention;
the aircraft is provided with a three-axis holder, the three-axis holder comprises a three-axis motor, and the three-axis holder is a first motor M1A second motor M2And a third motor M3The three-axis motors correspond to a pole pair number respectively, i.e., the first motor M1Corresponding to the first pole pair number N1The second motor M2Corresponding to the second logarithm of poles N2Said third motor M3Corresponding third pole logarithm N3The three-axis pan-tilt further comprises a base end and a tool end, wherein the base end and the tool end are both provided with a three-axis accelerometer, as shown in fig. 3, the initialization method of the joint angle comprises the following steps:
step S10, acquiring a first joint angle corresponding to the electrifying time of the first motor, a second electrical angle corresponding to the electrifying time of the second motor and a third electrical angle corresponding to the electrifying time of the third motor;
specifically, at the moment of power-on, obtain the first joint angle that first motor corresponds, specifically, obtain the first joint angle that first motor corresponds, include:
and determining a first joint angle corresponding to the first motor in a self-checking collision limiting mode. Specifically, a first joint angle theta corresponding to a first motor is determined by controlling the first motor to touch a mechanical limit of the first motorj1After the first joint angle corresponding to the first motor is determined, the second joint angle of the second motor and the third joint angle of the third motor can be calculated through the base end accelerometer and the tool end accelerometer.
Specifically, because the linear hall sensor is installed on the three-axis motor on the pan-tilt, it is used for detecting the magnetic field of the electric motor rotor to can calculate the first electric angle that first motor corresponds, the second electric angle that the second motor corresponds and the third electric angle that the third motor corresponds, and then acquire the first electric angle that first motor corresponds, the second electric angle that the second motor corresponds and the third electric angle that the third motor corresponds.
In an embodiment of the present invention, the method further comprises:
calculating an accelerometer module value of the base end and an accelerometer module value of the tool end;
and determining whether the cradle head is in a static state or not according to the accelerometer module value of the base end and the accelerometer module value of the tool end.
Specifically, when the accelerometer module value of the base end and the accelerometer module value of the tool end are both at a gravitational acceleration, it is determined that the cradle head is in a stationary state, and it can be understood that when the accelerometer module value of the base end and the accelerometer module value of the tool end are both near a gravitational acceleration, it may also be considered that the cradle head is in a stationary state.
Step S20: traversing second joint angles and third joint angles of the second motor and the third motor according to a second electrical angle and a second pole pair number of the second motor and a third electrical angle and a third pole pair number of the third motor, and determining joint angle combinations of the first motor, the second motor and the third motor;
specifically, each motor corresponds to a pole pair number, the first motor corresponds to a first pole pair number, the second motor corresponds to a second pole pair number, and the third motor corresponds to a third pole pair number, because the first joint angle of the first motor is determined in a self-checking collision limiting mode, and the pole pair number of the second motor is N2And the number of pole pairs of the third motor is N3The joint angles of the first motor, the second motor and the third motor have N2*N3A combination to determine a joint angle combination of the first, second and third motors.
It is understood that in said N2*N3In one combination, there are some unsatisfactory joint angle combinations that need to be filtered out, and therefore, after determining the joint angle combinations of the first motor, the second motor, and the third motor, the method further comprises: screening the joint angle combinations of the first motor, the second motor and the third motor, determining the screened joint angle combinations, specifically determining the joint angle combinations which are not in accordance with the mechanical limit of the holder, and excluding the joint angles which are not in accordance with the mechanical limit of the holder, for example: and the joint angles which are not in line with the mechanical limit of the holder in the second joint angle and the third joint angle, so that the screened joint angle combination is determined.
Step S30: determining an attitude quaternion corresponding to the joint angle combination and an attitude quaternion of the base end, and determining an attitude quaternion of the tool end according to the attitude quaternion of the base end and the attitude quaternion of the joint angle combination;
specifically, the determining the attitude quaternion corresponding to the joint angle combination includes:
determining an attitude quaternion corresponding to each joint angle combination, specifically, for a three-axis orthogonal pan/tilt head, according to a rotation sequence of "M1-M2-M3", that is, a rotation sequence of a base end-a tool end-an end, so as to determine a quaternion based on a base coordinate system, specifically, the quaternion based on the base coordinate system is expressed by the following formula (2):
Figure BDA0002627483060000111
wherein, thetaj1Is a first joint angle theta corresponding to the first motorj2A second joint angle, theta, corresponding to the second motorj3And a third joint angle corresponding to the third motor.
Specifically, referring back to fig. 4, fig. 4 is a detailed flowchart of step S30 in fig. 3;
as shown in fig. 4, the step S30: determining an attitude quaternion of the base end, comprising:
step S31: acquiring the reading of the triaxial accelerometer at the base end, fixing the yaw angle of the base end, and calculating the pitch angle and the roll angle of the base end according to the reading of the triaxial accelerometer at the base end;
specifically, the pitch angle and the roll angle of the base end are calculated according to the reading of the triaxial accelerometer of the base end by acquiring the reading of the triaxial accelerometer of the base end and fixing the yaw angle of the base end, that is, the yaw angle of the base end is set to 0. And calculating to obtain a pitch angle and a roll angle of the base end by fixing the yaw angle of the base end and reading of a triaxial accelerometer of the base end, thereby determining a group of Euler angles, and being beneficial to determining the attitude quaternion of the base end according to the group of Euler angles obtained by determination.
Step S32: and determining the attitude quaternion of the base end according to the yaw angle, the pitch angle and the roll angle of the base end.
Specifically, assume that the pitch angle in the euler angles is θpThe transverse rolling angle is thetarYaw angle θyAnd the sequence of their rotation angles is "ZXY", then according to the Euler angle, the calculation method for determining the attitude quaternion of the base end is as follows (3):
Figure BDA0002627483060000112
wherein, thetapTo a pitch angle, θrIs the roll angle, thetayIs yaw angle, θj1Is a first joint angle theta corresponding to the first motorj2A second joint angle, theta, corresponding to the second motorj3And a third joint angle corresponding to the third motor.
It can be understood that, when joint angle combinations of the first motor, the second motor and the third motor are screened, and joint angle combinations after screening are determined, the determining of the posture quaternion corresponding to the joint angle combinations includes:
and determining the posture quaternion corresponding to each screened joint angle combination.
Specifically, the determining the attitude quaternion of the tool end according to the attitude quaternion of the base end and the attitude quaternion of the joint angle combination includes:
and carrying out quaternion multiplication on the attitude quaternion of the base end and the attitude quaternion of the joint angle combination to determine the attitude quaternion of the tool end.
Specifically, assume that the attitude quaternion of the base end is P ═ P0 p1 p2 p3]The attitude quaternion of the joint angle combination is Q ═ Q0 q1 q2 q3]If the attitude quaternion of the tool end is R, the attitude quaternion of the tool end is a result obtained by the quaternion multiplication of the attitude quaternion of the base end and the attitude quaternion of the joint angle combination, that is, the result is
Figure BDA0002627483060000123
The quaternion multiplication is as follows (4):
Figure BDA0002627483060000121
that is, the calculated posture quaternion R of the tool end is [ R ═ R0 r1 r2 r3]。
Step S40: and determining the initialized joint angle combination of the three-axis motor by combining the readings of the three-axis accelerometer of the tool end according to the attitude quaternion of the tool end.
Specifically, referring back to fig. 5, fig. 5 is a detailed flowchart of step S40 in fig. 3;
as shown in fig. 5, the determining an initialized joint angle combination of the three-axis motor according to the attitude quaternion of the tool end and the reading of the three-axis accelerometer of the tool end includes:
step S41: determining a rotation matrix according to the attitude quaternion of the tool end;
specifically, assume that the attitude quaternion of the tool end is Q ═ Q0 q1 q2 q3]Then, the calculation mode of the corresponding rotation matrix M is as follows (5):
Figure BDA0002627483060000122
that is, the rotation matrix M is:
Figure BDA0002627483060000131
step S42: generating a gravity vector according to the reading of the triaxial accelerometer of the tool end;
specifically, the readings of the triaxial accelerometer at the tool end are obtained and taken as a gravity vector [ a ]x2 ay2 az2]。
Step S43: and determining the initialized joint angle combination of the three-axis motor according to the rotation matrix and the gravity vector.
Referring back to fig. 6, fig. 6 is a detailed flowchart of step S43 in fig. 5;
as shown in fig. 6, the determining an initialized joint angle combination of the three-axis motor according to the rotation matrix and the gravity vector includes:
step S431: acquiring a column vector of the rotation matrix;
specifically, a column vector of a last column of the rotation matrix, that is, a last column element of the rotation matrix is obtained.
Step S432: performing dot product operation on the column vector of the rotation matrix and the gravity vector to generate a plurality of dot product results;
specifically, the last column of elements [ a ] of the rotation matrix is obtainedx1 ay1 az1]The gravity vector [ a ]x2ay2 az2]And the last column element [ a ] of the rotation matrixx1 ay1 az1]And carrying out vector dot multiplication operation to obtain a dot multiplication result, and storing the dot multiplication result.
It is understood that there may be multiple readings of the tri-axial accelerometer, and thus multiple point multiplication results may occur, and therefore, a corresponding joint angle combination needs to be determined from the multiple point multiplication results as an initialized joint angle combination.
Step S433: and taking the first joint angle, the second joint angle and the third joint angle corresponding to the maximum value of the point multiplication result as the initialized joint angle combination of the three-axis motor.
Specifically, the maximum value of the dot product results is determined from all the calculated dot product results, and the second joint angle θ corresponding to the maximum value of the dot product resultsj2And a third joint angle thetaj3That is, the real joint angle of the second motor and the third motor is obtained, because the first joint angle θ corresponding to the first motor isj1Having determined, and thus determining, a first motor correspondence of the three-axis motorThe first initialized joint angle, the second initialized joint angle corresponding to the second motor and the third initialized joint angle corresponding to the third motor, so far, the joint angle initialization of the three-axis motor is completed.
In an embodiment of the present invention, an initialization method for a joint angle is provided, where the initialization method is applied to an aircraft, the aircraft is provided with a three-axis pan-tilt, the three-axis pan-tilt includes three-axis motors, which are a first motor, a second motor, and a third motor, respectively, the second motor corresponds to a second pole pair number, the third motor corresponds to a third pole pair number, the three-axis pan-tilt further includes a base end and a tool end, and the tool end is provided with a three-axis accelerometer, and the method includes: acquiring a first joint angle corresponding to the power-on time of the first motor, a second electrical angle corresponding to the power-on time of the second motor and a third electrical angle corresponding to the power-on time of a third motor; traversing second joint angles and third joint angles of the second motor and the third motor according to a second electrical angle and a second pole pair number of the second motor and a third electrical angle and a third pole pair number of the third motor, and determining joint angle combinations of the first motor, the second motor and the third motor; determining an attitude quaternion corresponding to the joint angle combination and an attitude quaternion of the base end, and determining an attitude quaternion of the tool end according to the attitude quaternion of the base end and the attitude quaternion of the joint angle combination; and determining the initialized joint angle combination of the three-axis motor by combining the readings of the three-axis accelerometer of the tool end according to the attitude quaternion of the tool end. By carrying out self-checking on the first motor and determining the initialized joint angle combination of the three-axis motor, the embodiment of the invention can improve the self-checking efficiency of the three-axis pan-tilt after being electrified.
Example two
Referring to fig. 7, fig. 7 is a schematic view of an apparatus for initializing a joint angle according to an embodiment of the present invention;
wherein, the initialization device 70 of the joint angle is applied to an aircraft, the aircraft is provided with a three-axis pan-tilt, the three-axis pan-tilt comprises three-axis motors, namely a first motor, a second motor and a third motor, the second motor corresponds to a second pole pair number, the third motor corresponds to a third pole pair number, the three-axis pan-tilt further comprises a base end and a tool end, the tool end is provided with a three-axis accelerometer,
the three-axis pan-tilt comprises a three-axis motor, which is a first motor M1A second motor M2And a third motor M3Wherein, first motor M is used for controlling the driftage axle arm of triaxial cloud platform and rotates, the second motor is used for controlling the roll axle arm of triaxial cloud platform and rotates, the third motor is used for controlling the pitch axle arm of triaxial cloud platform and rotates, and then realizes the control of triaxial cloud platform gesture, wherein, first motor M1Corresponding to the base end, the second motor M2Corresponding to the tool end, the third motor M3Corresponding to the end, wherein each motor corresponds to a pole pair number, assuming the first motor M1The corresponding first pole pair number is N1The second motor M2Corresponding second logarithmic logarithm of pole N2Said third motor M3The corresponding logarithm of the third pole is N3Wherein the electrical angle of the first motor at the time of electrifying is thetae1The electrical angle of the second motor is thetae2The electrical angle of the third motor is thetae3The first joint angle corresponding to the first motor is thetaj1The second joint angle corresponding to the second motor is thetaj2The third joint angle corresponding to the third motor is thetaj3Wherein, the corresponding relation between the joint angle and the electrical angle is respectively as the following formula (1):
Figure BDA0002627483060000151
wherein the wrap () function functions to limit the joint angle between [ -pi, pi ].
Due to the first motor M1Is parallel to the Z-axis direction of the base end, so that when the base end is horizontal, the first motor M is1The rotation of the base does not affect the value of gravity acting on the three-axis accelerometer at the tool end, so that the base end accelerometer and the tool end can be usedWith end accelerometers to calculate the second motor M2And a third motor M3The joint angle of (a).
As shown in fig. 7, the apparatus includes:
an obtaining unit 71, configured to obtain a first joint angle corresponding to a power-on time of the first motor, a second electrical angle corresponding to a power-on time of the second motor, and a third electrical angle corresponding to a power-on time of a third motor;
specifically, at the moment of power-on, obtain the first joint angle that first motor corresponds, specifically, obtain the first joint angle that first motor corresponds, include:
and determining a first joint angle corresponding to the first motor in a self-checking collision limiting mode. Specifically, a first joint angle theta corresponding to a first motor is determined by controlling the first motor to touch a mechanical limit of the first motorj1After the first joint angle corresponding to the first motor is determined, the second joint angle of the second motor and the third joint angle of the third motor can be calculated through the base end accelerometer and the tool end accelerometer.
Specifically, because the linear hall sensor is installed on the three-axis motor on the pan-tilt, it is used for detecting the magnetic field of the electric motor rotor to can calculate the first electric angle that first motor corresponds, the second electric angle that the second motor corresponds and the third electric angle that the third motor corresponds, and then acquire the first electric angle that first motor corresponds, the second electric angle that the second motor corresponds and the third electric angle that the third motor corresponds.
In an embodiment of the present invention, the method further comprises:
calculating an accelerometer module value of the base end and an accelerometer module value of the tool end;
and determining whether the cradle head is in a static state or not according to the accelerometer module value of the base end and the accelerometer module value of the tool end.
Specifically, when the accelerometer module value of the base end and the accelerometer module value of the tool end are both at a gravitational acceleration, it is determined that the cradle head is in a stationary state, and it can be understood that when the accelerometer module value of the base end and the accelerometer module value of the tool end are both near a gravitational acceleration, it may also be considered that the cradle head is in a stationary state.
The joint angle combination unit 72 is configured to traverse second joint angles and third joint angles of the second motor and the third motor according to a second electrical angle and a second pole pair number of the second motor and a third electrical angle and a third pole pair number of the third motor, and determine a joint angle combination of the first motor, the second motor, and the third motor;
specifically, each motor corresponds to a pole pair number, the first motor corresponds to a first pole pair number, the second motor corresponds to a second pole pair number, and the third motor corresponds to a third pole pair number, because the first joint angle of the first motor is determined in a self-checking collision limiting mode, and the pole pair number of the second motor is N2And the number of pole pairs of the third motor is N3The joint angles of the first motor, the second motor and the third motor have N2*N3A combination to determine a joint angle combination of the first, second and third motors.
It is understood that in said N2*N3In one combination, there are some unsatisfactory joint angle combinations that need to be filtered out, and therefore, after determining the joint angle combinations of the first motor, the second motor, and the third motor, the method further comprises: screening the joint angle combinations of the first motor, the second motor and the third motor, determining the screened joint angle combinations, specifically determining the joint angle combinations which are not in accordance with the mechanical limit of the holder, and excluding the joint angles which are not in accordance with the mechanical limit of the holder, for example: the joint angle which is not in accordance with the mechanical limit of the holder in the second joint angle and the third joint angle is determined, so that the screened joint angle is determinedThe joint angle combination of (1).
An attitude quaternion unit 73, configured to determine an attitude quaternion corresponding to the joint angle combination and an attitude quaternion at the base end, and determine an attitude quaternion at the tool end according to the attitude quaternion at the base end and the attitude quaternion at the joint angle combination;
specifically, the determining the attitude quaternion corresponding to the joint angle combination includes:
determining an attitude quaternion corresponding to each joint angle combination, specifically, for a three-axis orthogonal pan/tilt head, according to a rotation sequence of "M1-M2-M3", that is, a rotation sequence of a base end-a tool end-an end, so as to determine a quaternion based on a base coordinate system, specifically, the quaternion based on the base coordinate system is expressed by the following formula (2):
Figure BDA0002627483060000161
wherein, thetaj1Is a first joint angle theta corresponding to the first motorj2A second joint angle, theta, corresponding to the second motorj3And a third joint angle corresponding to the third motor.
And the initialized joint angle combination unit 74 is used for determining the initialized joint angle combination of the three-axis motor according to the posture quaternion of the tool end and by combining the readings of the three-axis accelerometer of the tool end.
In an embodiment of the present invention, the attitude quaternion unit is specifically configured to:
acquiring the reading of the triaxial accelerometer at the base end, fixing the yaw angle of the base end, and calculating the pitch angle and the roll angle of the base end according to the reading of the triaxial accelerometer at the base end;
specifically, the pitch angle and the roll angle of the base end are calculated according to the reading of the triaxial accelerometer of the base end by acquiring the reading of the triaxial accelerometer of the base end and fixing the yaw angle of the base end, that is, the yaw angle of the base end is set to 0. And calculating to obtain a pitch angle and a roll angle of the base end by fixing the yaw angle of the base end and reading of a triaxial accelerometer of the base end, thereby determining a group of Euler angles, and being beneficial to determining the attitude quaternion of the base end according to the group of Euler angles obtained by determination.
And determining the attitude quaternion of the base end according to the yaw angle, the pitch angle and the roll angle of the base end.
Specifically, assume that the pitch angle in the euler angles is θpThe transverse rolling angle is thetarYaw angle θyAnd the sequence of their rotation angles is "ZXY", then according to the Euler angle, the calculation method for determining the attitude quaternion of the base end is as follows (3):
Figure BDA0002627483060000171
wherein, thetapTo a pitch angle, θrIs the roll angle, thetayIs yaw angle, θj1Is a first joint angle theta corresponding to the first motorj2A second joint angle, theta, corresponding to the second motorj3And a third joint angle corresponding to the third motor.
It can be understood that, when joint angle combinations of the first motor, the second motor and the third motor are screened, and joint angle combinations after screening are determined, the determining of the posture quaternion corresponding to the joint angle combinations includes:
and determining the posture quaternion corresponding to each screened joint angle combination.
Specifically, the determining the attitude quaternion of the tool end according to the attitude quaternion of the base end and the attitude quaternion of the joint angle combination includes:
and carrying out quaternion multiplication on the attitude quaternion of the base end and the attitude quaternion of the joint angle combination to determine the attitude quaternion of the tool end.
Specifically, assume the attitude quaternion of the base endThe number P ═ P0 p1 p2 p3]The attitude quaternion of the joint angle combination is Q ═ Q0 q1 q2 q3]If the attitude quaternion of the tool end is R, the attitude quaternion of the tool end is a result obtained by the quaternion multiplication of the attitude quaternion of the base end and the attitude quaternion of the joint angle combination, that is, the result is
Figure BDA0002627483060000184
The quaternion multiplication is as follows (4):
Figure BDA0002627483060000181
that is, the calculated posture quaternion R of the tool end is [ R ═ R0 r1 r2 r3]。
In an embodiment of the present invention, the apparatus further includes:
and the screening unit is used for screening joint angle combinations of the first motor, the second motor and the third motor, determining the screened joint angle combinations, and determining the posture quaternion corresponding to the joint angle combinations.
In an embodiment of the present invention, the obtaining unit is specifically configured to:
and determining a first joint angle corresponding to the first motor in a self-checking collision limiting mode.
In an embodiment of the present invention, the apparatus further includes:
the static state unit is used for calculating an accelerometer module value of the base end and an accelerometer module value of the tool end;
and determining whether the cradle head is in a static state or not according to the accelerometer module value of the base end and the accelerometer module value of the tool end.
In an embodiment of the present invention, the attitude quaternion unit is specifically configured to:
and carrying out quaternion multiplication on the attitude quaternion of the base end and the attitude quaternion of the joint angle combination to determine the attitude quaternion of the tool end.
Specifically, assume that the attitude quaternion of the tool end is Q ═ Q0 q1 q2 q3]Then, the calculation mode of the corresponding rotation matrix M is as follows (5):
Figure BDA0002627483060000182
that is, the rotation matrix M is:
Figure BDA0002627483060000183
in an embodiment of the present invention, the initialized joint angle combining unit is specifically configured to:
acquiring a column vector of the rotation matrix;
specifically, a column vector of a last column of the rotation matrix, that is, a last column element of the rotation matrix is obtained.
Performing dot product operation on the column vector of the rotation matrix and the gravity vector to generate a plurality of dot product results;
specifically, the last column of elements [ a ] of the rotation matrix is obtainedx1 ay1 az1]The gravity vector [ a ]x2ay2 az2]And the last column element [ a ] of the rotation matrixx1 ay1 az1]And carrying out vector dot multiplication operation to obtain a dot multiplication result, and storing the dot multiplication result.
It is understood that there may be multiple readings of the tri-axial accelerometer, and thus multiple point multiplication results may occur, and therefore, a corresponding joint angle combination needs to be determined from the multiple point multiplication results as an initialized joint angle combination.
And taking the first joint angle, the second joint angle and the third joint angle corresponding to the maximum value of the point multiplication result as the initialized joint angle combination of the three-axis motor.
Specifically, the dot product result is determined from all the calculated dot product resultsA second joint angle θ corresponding to a maximum value of the dot product resultj2And a third joint angle thetaj3That is, the real joint angle of the second motor and the third motor is obtained, because the first joint angle θ corresponding to the first motor isj1And determining a first initialized joint angle corresponding to a first motor, a second initialized joint angle corresponding to a second motor and a third initialized joint angle corresponding to a third motor of the three-axis motor, so far, completing the initialization of the joint angles of the three-axis motor.
It should be noted that the above-mentioned apparatus can execute the method provided by the embodiments of the present application, and has corresponding functional modules and beneficial effects for executing the method. For technical details which are not described in detail in the device embodiments, reference is made to the methods provided in the embodiments of the present application.
In an embodiment of the present invention, an initialization apparatus for a joint angle is provided, which is applied to an aircraft, where the aircraft is provided with a three-axis pan-tilt, the three-axis pan-tilt includes three-axis motors, respectively a first motor, a second motor, and a third motor, the second motor corresponds to a second pole pair number, the third motor corresponds to a third pole pair number, the three-axis pan-tilt further includes a base end and a tool end, the tool end is provided with a three-axis accelerometer, and the apparatus includes: the acquisition unit is used for acquiring a first joint angle corresponding to the electrifying time of the first motor, a second electrical angle corresponding to the electrifying time of the second motor and a third electrical angle corresponding to the electrifying time of the third motor; the joint angle combination unit is used for traversing second joint angles and third joint angles of the second motor and the third motor according to a second electric angle and a second pole logarithm of the second motor and a third electric angle and a third pole logarithm of the third motor, and determining joint angle combinations of the first motor, the second motor and the third motor; the attitude quaternion unit is used for determining an attitude quaternion corresponding to the joint angle combination and an attitude quaternion of the base end, and determining an attitude quaternion of the tool end according to the attitude quaternion of the base end and the attitude quaternion of the joint angle combination; and the initialized joint angle combination unit is used for determining the initialized joint angle combination of the three-axis motor according to the attitude quaternion of the tool end and by combining the reading of the three-axis accelerometer of the tool end. By carrying out self-checking on the first motor and determining the initialized joint angle combination of the three-axis motor, the embodiment of the invention can improve the self-checking efficiency of the three-axis pan-tilt after being electrified.
Referring to fig. 8, fig. 8 is a schematic diagram of a hardware structure of an aircraft according to an embodiment of the present invention. The aircraft may be an Unmanned Aerial Vehicle (UAV), an unmanned spacecraft, or other electronic devices.
As shown in fig. 8, the aircraft 800 includes one or more processors 801 and memory 802. In fig. 8, one processor 801 is taken as an example.
The processor 801 and the memory 802 may be connected by a bus or other means, such as by a bus in fig. 8.
The memory 802, which is a non-volatile computer-readable storage medium, may be used to store non-volatile software programs, non-volatile computer-executable programs, and modules, such as units corresponding to an initialization method of a joint angle in an embodiment of the present invention (for example, the respective modules or units described in fig. 7). The processor 801 executes various functional applications and data processing of the initialization method of the joint angle by running the nonvolatile software program, instructions, and modules stored in the memory 802, that is, the functions of the initialization method of the joint angle of the above-described method embodiment and the respective modules and units of the above-described apparatus embodiment are realized.
The memory 802 may include high speed random access memory and may also include non-volatile memory, such as at least one magnetic disk storage device, flash memory device, or other non-volatile solid state storage device. In some embodiments, the memory 802 optionally includes memory located remotely from the processor 801, which may be connected to the processor 801 via a network. Examples of such networks include, but are not limited to, the internet, intranets, local area networks, mobile communication networks, and combinations thereof.
The modules are stored in the memory 802 and, when executed by the one or more processors 801, perform the method of initializing joint angles in any of the method embodiments described above, e.g., performing the various steps shown in fig. 3-6 described above; the functions of the respective modules or units described in fig. 7 can also be implemented.
Referring to fig. 9 and 10, the aircraft 800 further includes a power system 803, the power system 803 is used for providing flight power for the aircraft, and the power system 803 is connected to the processor 801. The power system 803 includes: the electric control device comprises a driving motor 8031 and an electric control 8032, wherein the electric control 8032 is electrically connected with the driving motor 8031 and is used for controlling the driving motor 8031. Specifically, the electric tilt 8032 executes the above-mentioned method for initializing the joint angle based on the processor 801 to generate a control instruction, and controls the driving motor 8031 through the control instruction.
The aircraft 800 can execute the method for initializing the joint angle provided by the first embodiment of the invention, and has corresponding functional modules and beneficial effects of the execution method. For technical details that are not described in detail in the aircraft embodiment, reference may be made to the method for initializing the joint angle provided in the first embodiment of the present invention.
An embodiment of the present invention provides a computer program product comprising a computer program stored on a non-transitory computer-readable storage medium, the computer program comprising program instructions which, when executed by a computer, cause the computer to perform the method of initializing a joint angle as described above. For example, the method steps S10 to S40 in fig. 3 described above are performed.
Embodiments of the present invention also provide a non-transitory computer storage medium storing computer-executable instructions, which are executed by one or more processors, such as one of the processors 801 in fig. 8, to enable the one or more processors to perform the method for initializing the joint angle in any of the above-described method embodiments, such as performing the above-described steps shown in fig. 3 to 6; the functions of the respective modules or units shown in fig. 7 can also be realized.
The above-described embodiments of the apparatus or device are merely illustrative, wherein the unit modules described as separate parts may or may not be physically separate, and the parts displayed as module units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network module units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the solution of the present embodiment.
Through the above description of the embodiments, those skilled in the art will clearly understand that each embodiment can be implemented by software plus a general hardware platform, and certainly can also be implemented by hardware. Based on such understanding, the technical solutions mentioned above may be embodied in the form of a software product, which may be stored in a computer-readable storage medium, such as ROM/RAM, magnetic disk, optical disk, etc., and includes instructions for causing a computer device (which may be a personal computer, a server, or a network device) to execute the method according to each embodiment or some parts of the embodiments.
Finally, it should be noted that: the above examples are only intended to illustrate the technical solution of the present invention, but not to limit it; within the idea of the invention, also technical features in the above embodiments or in different embodiments may be combined, steps may be implemented in any order, and there are many other variations of the different aspects of the invention as described above, which are not provided in detail for the sake of brevity; although the present invention has been described in detail with reference to the foregoing embodiments, it will be understood by those of ordinary skill in the art that: the technical solutions described in the foregoing embodiments may still be modified, or some technical features may be equivalently replaced; and the modifications or the substitutions do not make the essence of the corresponding technical solutions depart from the scope of the technical solutions of the embodiments of the present application.

Claims (10)

1. The utility model provides an initialization method of joint angle, is applied to the aircraft, the aircraft is provided with the triaxial cloud platform, a serial communication port, the triaxial cloud platform includes the triaxial motor, is first motor, second motor and third motor respectively, the second motor corresponds the second pole logarithm, the third motor corresponds the third pole logarithm, the triaxial cloud platform still includes base end and instrument end, the instrument end is provided with the triaxial accelerometer, the method includes:
acquiring a first joint angle corresponding to the power-on time of the first motor, a second electrical angle corresponding to the power-on time of the second motor and a third electrical angle corresponding to the power-on time of a third motor;
traversing second joint angles and third joint angles of the second motor and the third motor according to a second electrical angle and a second pole pair number of the second motor and a third electrical angle and a third pole pair number of the third motor, and determining joint angle combinations of the first motor, the second motor and the third motor;
determining an attitude quaternion corresponding to the joint angle combination and an attitude quaternion of the base end, and determining an attitude quaternion of the tool end according to the attitude quaternion of the base end and the attitude quaternion of the joint angle combination;
and determining the initialized joint angle combination of the three-axis motor by combining the readings of the three-axis accelerometer of the tool end according to the attitude quaternion of the tool end.
2. The method of claim 1, wherein the base end is provided with a three-axis accelerometer, and wherein determining the attitude quaternion of the base end comprises:
acquiring the reading of the triaxial accelerometer at the base end, fixing the yaw angle of the base end, and calculating the pitch angle and the roll angle of the base end according to the reading of the triaxial accelerometer at the base end;
and determining the attitude quaternion of the base end according to the yaw angle, the pitch angle and the roll angle of the base end.
3. The method of claim 1, wherein after determining the joint angle combination of the first, second, and third motors, the method further comprises: screening joint angle combinations of the first motor, the second motor and the third motor, determining the screened joint angle combinations, and determining attitude quaternions corresponding to the joint angle combinations, wherein the attitude quaternions comprise:
and determining the posture quaternion corresponding to each screened joint angle combination.
4. The method of claim 1, wherein the obtaining a first joint angle corresponding to the first motor at a power-on time comprises:
and determining a first joint angle corresponding to the first motor in a self-checking collision limiting mode.
5. The method of claim 1, further comprising:
calculating an accelerometer module value of the base end and an accelerometer module value of the tool end;
and determining whether the cradle head is in a static state or not according to the accelerometer module value of the base end and the accelerometer module value of the tool end.
6. The method of claim 1, wherein determining the attitude quaternion of the tool end from the attitude quaternion of the base end and the attitude quaternion of the joint angle combination comprises:
and carrying out quaternion multiplication on the attitude quaternion of the base end and the attitude quaternion of the joint angle combination to determine the attitude quaternion of the tool end.
7. The method of claim 1, wherein determining an initialized joint angle combination for the three-axis motor from the attitude quaternion of the tool end in combination with readings from a three-axis accelerometer of the tool end comprises:
determining a rotation matrix according to the attitude quaternion of the tool end;
generating a gravity vector according to the reading of the triaxial accelerometer of the tool end;
and determining the initialized joint angle combination of the three-axis motor according to the rotation matrix and the gravity vector.
8. The method of claim 7, wherein determining an initialized joint angle combination for the three-axis motor from the rotation matrix and the gravity vector comprises:
acquiring a column vector of the rotation matrix;
performing dot product operation on the column vector of the rotation matrix and the gravity vector to generate a plurality of dot product results;
and taking the first joint angle, the second joint angle and the third joint angle corresponding to the maximum value of the point multiplication result as the initialized joint angle combination of the three-axis motor.
9. The utility model provides an initialization device of joint angle, is applied to the aircraft, the aircraft is provided with the triaxial cloud platform, a serial communication port, the triaxial cloud platform includes the triaxial motor, is first motor, second motor and third motor respectively, the second motor corresponds the second pole logarithm, the third motor corresponds the third pole logarithm, the triaxial cloud platform still includes base end and instrument end, the instrument end is provided with the triaxial accelerometer, the device includes:
the acquisition unit is used for acquiring a first joint angle corresponding to the electrifying time of the first motor, a second electrical angle corresponding to the electrifying time of the second motor and a third electrical angle corresponding to the electrifying time of the third motor;
the joint angle combination unit is used for traversing second joint angles and third joint angles of the second motor and the third motor according to a second electric angle and a second pole logarithm of the second motor and a third electric angle and a third pole logarithm of the third motor, and determining joint angle combinations of the first motor, the second motor and the third motor;
the attitude quaternion unit is used for determining an attitude quaternion corresponding to the joint angle combination and an attitude quaternion of the base end, and determining an attitude quaternion of the tool end according to the attitude quaternion of the base end and the attitude quaternion of the joint angle combination;
and the initialized joint angle combination unit is used for determining the initialized joint angle combination of the three-axis motor according to the attitude quaternion of the tool end and by combining the reading of the three-axis accelerometer of the tool end.
10. An aircraft, characterized in that it comprises:
a body;
the machine arm is connected with the machine body;
the power device is arranged on the fuselage and/or the horn and is used for providing flying power for the aircraft;
the three-axis holder is arranged on the machine body and comprises a three-axis motor, a first motor, a second motor and a third motor, and the three-axis holder further comprises a base end and a tool end;
the flight controller is arranged on the machine body;
wherein the flight controller includes:
at least one processor; and the number of the first and second groups,
a memory communicatively coupled to the at least one processor; wherein the content of the first and second substances,
the memory stores instructions executable by the at least one processor to enable the at least one processor to perform the method of initializing a joint angle of any one of claims 1-8.
CN202010801302.4A 2020-08-11 2020-08-11 Method and device for initializing joint angle and aircraft Pending CN111977006A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010801302.4A CN111977006A (en) 2020-08-11 2020-08-11 Method and device for initializing joint angle and aircraft

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010801302.4A CN111977006A (en) 2020-08-11 2020-08-11 Method and device for initializing joint angle and aircraft

Publications (1)

Publication Number Publication Date
CN111977006A true CN111977006A (en) 2020-11-24

Family

ID=73433848

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010801302.4A Pending CN111977006A (en) 2020-08-11 2020-08-11 Method and device for initializing joint angle and aircraft

Country Status (1)

Country Link
CN (1) CN111977006A (en)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113305837A (en) * 2021-05-25 2021-08-27 乐聚(深圳)机器人技术有限公司 Method and device for determining deviation information of robot, processing equipment and medium
CN113815875A (en) * 2021-09-30 2021-12-21 北京远度互联科技有限公司 Pod control method and device, pod, unmanned aerial vehicle and medium
CN113928581A (en) * 2021-09-30 2022-01-14 北京远度互联科技有限公司 Pod control method and device, pod, unmanned aerial vehicle and medium

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101926086A (en) * 2008-07-31 2010-12-22 爱信艾达株式会社 Rotating electric device control system and vehicle drive system
EP3502844A1 (en) * 2017-12-21 2019-06-26 Thales Method and system for resetting piloting symbology of an aircraft on a worn head-up display in accordance with the real world outside via an svs synthetic vision system
KR20190084896A (en) * 2018-01-08 2019-07-17 변정태 System for controlling drone inculding gimbal and drone device using voice and method for controlling group drone
CN110446885A (en) * 2018-01-06 2019-11-12 深圳市大疆创新科技有限公司 The control method and its holder and storage device of nonopiate holder
JP2020030204A (en) * 2018-08-21 2020-02-27 エスゼット ディージェイアイ テクノロジー カンパニー リミテッドＳｚ Ｄｊｉ Ｔｅｃｈｎｏｌｏｇｙ Ｃｏ．，Ｌｔｄ Distance measurement method, program, distance measurement system and movable object

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101926086A (en) * 2008-07-31 2010-12-22 爱信艾达株式会社 Rotating electric device control system and vehicle drive system
EP3502844A1 (en) * 2017-12-21 2019-06-26 Thales Method and system for resetting piloting symbology of an aircraft on a worn head-up display in accordance with the real world outside via an svs synthetic vision system
CN110446885A (en) * 2018-01-06 2019-11-12 深圳市大疆创新科技有限公司 The control method and its holder and storage device of nonopiate holder
KR20190084896A (en) * 2018-01-08 2019-07-17 변정태 System for controlling drone inculding gimbal and drone device using voice and method for controlling group drone
JP2020030204A (en) * 2018-08-21 2020-02-27 エスゼット ディージェイアイ テクノロジー カンパニー リミテッドＳｚ Ｄｊｉ Ｔｅｃｈｎｏｌｏｇｙ Ｃｏ．，Ｌｔｄ Distance measurement method, program, distance measurement system and movable object

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113305837A (en) * 2021-05-25 2021-08-27 乐聚(深圳)机器人技术有限公司 Method and device for determining deviation information of robot, processing equipment and medium
CN113305837B (en) * 2021-05-25 2022-05-17 乐聚(深圳)机器人技术有限公司 Method and device for determining deviation information of robot, processing equipment and medium
CN113815875A (en) * 2021-09-30 2021-12-21 北京远度互联科技有限公司 Pod control method and device, pod, unmanned aerial vehicle and medium
CN113928581A (en) * 2021-09-30 2022-01-14 北京远度互联科技有限公司 Pod control method and device, pod, unmanned aerial vehicle and medium
CN113928581B (en) * 2021-09-30 2022-08-23 北京远度互联科技有限公司 Pod control method and device, pod, unmanned aerial vehicle and medium
CN113815875B (en) * 2021-09-30 2022-10-11 北京远度互联科技有限公司 Pod control method and device, pod, unmanned aerial vehicle and medium

Similar Documents

Publication Publication Date Title
CN108549399B (en) Aircraft yaw angle correction method and device and aircraft
WO2019223270A1 (en) Method and apparatus for estimating angle and angular velocity of electric motor of gimbal, and gimbal and aerial vehicle
CN109000612B (en) Equipment angle estimation method and device, camera shooting assembly and aircraft
CN111977006A (en) Method and device for initializing joint angle and aircraft
CN108235702B (en) Cloud deck, unmanned aerial vehicle and control method thereof
CN110440805B (en) Method and device for fusing yaw angles and aircraft
CN110377058B (en) Aircraft yaw angle correction method and device and aircraft
CN107872180B (en) Method and device for detecting position of motor rotor and electronic equipment
CN203705964U (en) Stable closed-loop control device of airborne 3-DOF pan-tilt
JP6114865B1 (en) MOBILE BODY, MOBILE BODY CONTROL METHOD, AND PROGRAM
CN104508346A (en) Carrier of non-orthogonal shafts
CN110487277B (en) Method and device for fusing yaw angles and aircraft
CN105045293B (en) Cloud platform control method, outer carrier control method and holder
CN110998266B (en) Data processing apparatus, drive control apparatus, data processing method, and storage medium
CN110568860A (en) Unmanned aerial vehicle return method and device and unmanned aerial vehicle
CN208299634U (en) A kind of motor, holder and unmanned vehicle
CN112650265B (en) Control method, device, equipment and aircraft
WO2019134154A1 (en) Control method for non-orthogonal tripod head, tripod head thereof, and storage device
US20230205206A1 (en) Obstacle avoidance method, apparatus and unmanned aerial vehicle
CN109844394B (en) Cloud deck control method and cloud deck
WO2020062089A1 (en) Magnetic sensor calibration method and movable platform
CN108475066B (en) Unmanned aerial vehicle attitude calculation method, flight controller and unmanned aerial vehicle
WO2020042159A1 (en) Rotation control method and apparatus for gimbal, control device, and mobile platform
CN111357273A (en) Shooting parameter adjusting method, shooting equipment and movable platform
CN112236734B (en) Load stabilization device, control method thereof and computer readable storage medium

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
CB02 Change of applicant information

Address after: 518055 Guangdong city of Shenzhen province Nanshan District Xili Street Xueyuan Road No. 1001 Chi Yuen Building 9 layer B1

Applicant after: Shenzhen daotong intelligent Aviation Technology Co.,Ltd.

Address before: 518055 Guangdong city of Shenzhen province Nanshan District Xili Street Xueyuan Road No. 1001 Chi Yuen Building 9 layer B1

Applicant before: AUTEL ROBOTICS Co.,Ltd.

CB02 Change of applicant information